diff --git a/.gitignore b/.gitignore
index 3fc83b58662e303434f5d61f0d485163d7d22154..0979ed0c958d230c5a5b7f226a92e289446f7851 100755
--- a/.gitignore
+++ b/.gitignore
@@ -17,8 +17,25 @@ dfall_ws/src/CMakeLists.txt
TAGS
build-*
-Crazyflie.db
# For LaTeX files in the wiki
wiki/latex/*
!wiki/latex/*.tex
+
+# NOTES:
+# > To keep track of configuration-type files that
+# are part of the repository, BUT for whcih
+# changes are NOT tracked.
+# > These files are "skipped" from tracking using
+# the following syntax:
+# >> git update-index --skip-worktree <FILENAME>
+# > The files that should be "skipped" are:
+# >> dfall_ws/src/dfall_pkg/launch/Config.sh
+# >> dfall_ws/src/dfall_pkg/param/Crazyflie.db
+# > To return these files to normal tracking use
+# the following syntax:
+# >> git update-index --no-skip-worktree <FILENAME>
+# > "Skipping" is a setting local to your copy of the
+# repository because ".git ignoring" a file already
+# in the repository does NOT stop changes from being
+# tracked
diff --git a/crazyflie-firmware/README.md b/crazyflie-firmware/README.md
index c6e4a032ea30cd55ee9d62052c9f767107f2c9f4..40219f33fd4da81a2821048a67f42c3ac2fc6a08 100644
--- a/crazyflie-firmware/README.md
+++ b/crazyflie-firmware/README.md
@@ -6,7 +6,7 @@ https://github.com/bitcraze/crazyflie-firmware
However, it is easiest to locate the official realease versions of the firmware from here:<br>
https://github.com/bitcraze/crazyflie-release/releases
-The complete instructions for applying the patch are given separately for each firmware version.
+The complete instructions for applying the patch are given separately for each firmware version (currently only one firmware version).
@@ -38,9 +38,11 @@ To apply the patch, use the following sequence of commands:
`git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git`<br>
`cd crazyflie-firmware`<br>
`git checkout e27cd170b7eb8be7ed1cbbc9f5622b469d335d97`<br>
+Note that it is also possible to checkout this commit using the commit tag, i.e., using `git checkout 2020.02`
+
**Step 2:** Apply the patch:<br>
-`git apply <folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2019-09/firmware_modifications_for_version_2019-09.patch`<br>
+`git apply <folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-20/firmware_modifications_for_version_2020-20.patch`<br>
Now the repository is ready to compile. To do this, first make sure that you have
installed the necessary toolchain as described above ([Install Compilation Toolchain](#install-compilation-toolchain)).
@@ -63,47 +65,4 @@ The pre-compiled binary files are already included in this repository. To update
`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin`<br>
To update the firmware of both the main processor and the nrf processor, select the file:<br>
-`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip`<br>
-
-
-
-
-
-## Firware Version 2017-06
-
-The hash of the exact commit to which this patch applies is:<br>
-`f4d21213d7ce37d7ed7338fad0f9a94a96cec191`
-As commited on Mon Feb 3 15:04:52 2020 +0100
-
-To apply the patch, use the following sequence of commands:
-
-**Step 1:** Clone the repository and checkout the appropriate commit:<br>
-`git clone --recursive https://github.com/bitcraze/crazyflie-firmware.git`<br>
-`cd crazyflie-firmware`<br>
-`git checkout f4d21213d7ce37d7ed7338fad0f9a94a96cec191`<br>
-
-**Step 2:** Apply the patch:<br>
-`git apply <folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch`<br>
-
-Now the repository is ready to compile. To do this, first make sure that you have
-installed the necessary toolchain as described above ([Install Compilation Toolchain](#install-compilation-toolchain)).
-
-**Step 3:** In the `crazyflie-firmware` folder where you applied the patch, compile the firmware using the following command:<br>
-`make PLATFORM=CF2`
-
-If everything is successful, you will see something like this:<br>
-
-<img src="success_building_for_version_2017-06.png" style="width: 400px;"/> <br><br>
-
-This should have created the binary file `cf2.bin`.
-
-**Step 4:** upload the binary to the crazyflie processor by following the instructions given here:<br>
-https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/#update-fw<br>
-Instructions are also given in the wiki of this repository under:
-`/wiki/setup.md#firmware`
-
-The pre-compiled binary files are already included in this repository. To update the firmware of just the main processor, select the file:
-`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin`<br>
-
-To update the firmware of both the main processor and the nrf processor, select the file:<br>
-`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip`<br>
\ No newline at end of file
+`<folder_to_dfall-system_repository>/crazyflie_firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip`<br>
\ No newline at end of file
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf1-2017.06.bin b/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf1-2017.06.bin
deleted file mode 100644
index 8fe23d6d7142ee791214be735a5bbe51bdea8024..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf1-2017.06.bin and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin b/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin
deleted file mode 100755
index 5f2a63f7b230e616de18d2fe1f5df25f36b6a7b3..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2-2017.06_withDfallPatch.bin and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2_nrf-2017.05.bin b/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2_nrf-2017.05.bin
deleted file mode 100644
index e73ab57124abde79c75fe14a754a7ad40f0a7c59..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/cf2_nrf-2017.05.bin and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip b/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip
deleted file mode 100644
index 79ac384102115983868f12721c3e79117775817d..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware-cf2-2017.06_withDfallPatch.zip and /dev/null differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch b/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch
deleted file mode 100644
index 0718a5654065db8dff377ef1f1a7568a524cc88d..0000000000000000000000000000000000000000
--- a/crazyflie-firmware/crazyflie-firmware-version-2017-06/firmware_modifications_for_version_2017-06.patch
+++ /dev/null
@@ -1,435 +0,0 @@
-diff --git a/src/modules/interface/attitude_controller.h b/src/modules/interface/attitude_controller.h
-index ad23ef0..f79b7d1 100644
---- a/src/modules/interface/attitude_controller.h
-+++ b/src/modules/interface/attitude_controller.h
-@@ -63,6 +63,11 @@ void attitudeControllerResetRollAttitudePID(void);
- */
- void attitudeControllerResetPitchAttitudePID(void);
-
-+/**
-+ * Reset controller yaw attitude PID
-+ */
-+void attitudeControllerResetYawAttitudePID(void);
-+
- /**
- * Reset controller roll, pitch and yaw PID's.
- */
-diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
-index 418cc0b..30d031b 100644
---- a/src/modules/interface/stabilizer_types.h
-+++ b/src/modules/interface/stabilizer_types.h
-@@ -144,6 +144,10 @@ typedef struct control_s {
- int16_t pitch;
- int16_t yaw;
- float thrust;
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
- } control_t;
-
- typedef enum mode_e {
-@@ -152,12 +156,24 @@ typedef enum mode_e {
- modeVelocity
- } stab_mode_t;
-
-+typedef enum pps_mode_e {
-+ modeDisabled = 0,
-+ modeMotors,
-+ modeRate,
-+ modeAngle
-+} pps_mode_t;
-+
- typedef struct setpoint_s {
- uint32_t timestamp;
-
- attitude_t attitude;
- attitude_t attitudeRate;
- float thrust;
-+ pps_mode_t pps_mode;
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
- point_t position;
- velocity_t velocity;
- bool velocity_body;
-diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c
-index 8b5f4aa..f16288f 100644
---- a/src/modules/src/attitude_pid_controller.c
-+++ b/src/modules/src/attitude_pid_controller.c
-@@ -142,9 +142,14 @@ void attitudeControllerResetRollAttitudePID(void)
-
- void attitudeControllerResetPitchAttitudePID(void)
- {
-- pidReset(&pidRoll);
-+ pidReset(&pidPitch);
- }
-
-+void attitudeControllerResetYawAttitudePID(void)
-+{
-+ pidReset(&pidYaw);
-+}
-+
- void attitudeControllerResetAllPID(void)
- {
- pidReset(&pidRoll);
-diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
-index 0e5e62d..516315a 100644
---- a/src/modules/src/controller_pid.c
-+++ b/src/modules/src/controller_pid.c
-@@ -51,9 +51,13 @@ void stateController(control_t *control, setpoint_t *setpoint,
- }
-
- if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
-- positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ if(setpoint->pps_mode != modeMotors && setpoint->pps_mode != modeRate && setpoint->pps_mode != modeAngle)
-+ {
-+ positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ }
- }
-
-+
- if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
- // Switch between manual and automatic position control
- if (setpoint->mode.z == modeDisable) {
-@@ -63,7 +67,7 @@ void stateController(control_t *control, setpoint_t *setpoint,
- attitudeDesired.roll = setpoint->attitude.roll;
- attitudeDesired.pitch = setpoint->attitude.pitch;
- }
--
-+ /* if everything is modeVelocity, skip this */
- attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
- attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
- &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
-@@ -80,6 +84,12 @@ void stateController(control_t *control, setpoint_t *setpoint,
- attitudeControllerResetPitchAttitudePID();
- }
-
-+ //added for PPSSafeController to make the Onboard Controller using the external YawRate. Pure speed controller, no position/500 small steps
-+ if (setpoint->mode.yaw == modeVelocity) {
-+ rateDesired.yaw = (-1) * setpoint->attitudeRate.yaw;
-+ attitudeControllerResetYawAttitudePID();
-+ }
-+
- // TODO: Investigate possibility to subtract gyro drift.
- attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
- rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
-@@ -100,19 +110,76 @@ void stateController(control_t *control, setpoint_t *setpoint,
- control->thrust = actuatorThrust;
- }
-
-- if (control->thrust == 0)
-+ if(setpoint->pps_mode == modeMotors)
- {
-- control->thrust = 0;
-- control->roll = 0;
-- control->pitch = 0;
-- control->yaw = 0;
--
-- attitudeControllerResetAllPID();
-- positionControllerResetAllPID();
--
-- // Reset the calculated YAW angle for rate control
-- attitudeDesired.yaw = state->attitude.yaw;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
-+ {
-+ control->cmd1 = 0;
-+ control->cmd2 = 0;
-+ control->cmd3 = 0;
-+ control->cmd4 = 0;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+
-+ attitudeControllerResetAllPID();
-+ positionControllerResetAllPID();
-+
-+ // Reset the calculated YAW angle for rate control
-+ attitudeDesired.yaw = state->attitude.yaw;
-+ }
-+ }
-+ else if(setpoint->pps_mode == modeRate || setpoint->pps_mode == modeAngle)
-+ {
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
-+ {
-+ control->cmd1 = 0;
-+ control->cmd2 = 0;
-+ control->cmd3 = 0;
-+ control->cmd4 = 0;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+
-+ attitudeControllerResetAllPID();
-+ positionControllerResetAllPID();
-+
-+ // Reset the calculated YAW angle for rate control
-+ attitudeDesired.yaw = state->attitude.yaw;
-+ }
- }
-+ else /* pps_mode == modeDisabled */
-+ {
-+ control->cmd1 = control->thrust;
-+ control->cmd2 = control->thrust;
-+ control->cmd3 = control->thrust;
-+ control->cmd4 = control->thrust;
-+ if (control->thrust == 0)
-+ {
-+ control->thrust = 0;
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+
-+ attitudeControllerResetAllPID();
-+ positionControllerResetAllPID();
-+
-+ // Reset the calculated YAW angle for rate control
-+ attitudeDesired.yaw = state->attitude.yaw;
-+ }
-+ }
-+
- }
-
-
-diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
-index 993dead..4370101 100644
---- a/src/modules/src/crtp_commander_generic.c
-+++ b/src/modules/src/crtp_commander_generic.c
-@@ -34,6 +34,8 @@
- #include "num.h"
- #include "FreeRTOS.h"
-
-+/* #include "motors.h" */
-+
- /* The generic commander format contains a packet type and data that has to be
- * decoded into a setpoint_t structure. The aim is to make it future-proof
- * by easily allowing the addition of new packets for future use cases.
-@@ -67,6 +69,9 @@ enum packet_type {
- cppmEmuType = 3,
- altHoldType = 4,
- hoverType = 5,
-+ PPSMotorsType = 8,
-+ PPSRateType = 9,
-+ PPSAngleType = 10,
- };
-
- /* ---===== 2 - Decoding functions =====--- */
-@@ -292,6 +297,152 @@ static void hoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, s
- setpoint->velocity_body = true;
- }
-
-+struct PPSMotorsPacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+} __attribute__((packed));
-+
-+static void PPSMotorsDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct PPSMotorsPacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct PPSMotorsPacket_s));
-+
-+ /* we dont use position controller onboard */
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
-+
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ /* we dont use rate controller onboard either */
-+ setpoint->mode.roll = modeDisable;
-+ setpoint->mode.pitch = modeDisable;
-+ setpoint->mode.yaw = modeDisable;
-+
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
-+
-+ /* update cmds */
-+
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ setpoint->pps_mode = modeMotors;
-+}
-+
-+struct PPSRatePacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+ float roll_rate;
-+ float pitch_rate;
-+ float yaw_rate;
-+} __attribute__((packed));
-+
-+static void PPSRateDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct PPSRatePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct PPSRatePacket_s));
-+
-+ /* we dont use position controller onboard */
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
-+
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ /* we use rate controller onboard either */
-+ setpoint->mode.roll = modeVelocity;
-+ setpoint->mode.pitch = modeVelocity;
-+ setpoint->mode.yaw = modeVelocity;
-+
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ setpoint->attitudeRate.roll = values->roll_rate;
-+ setpoint->attitudeRate.pitch = values->pitch_rate;
-+ setpoint->attitudeRate.yaw = values->yaw_rate;
-+
-+ /* update cmds */
-+
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ setpoint->pps_mode = modeRate;
-+}
-+
-+struct PPSAnglePacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+ float roll;
-+ float pitch;
-+ float yaw;
-+} __attribute__((packed));
-+
-+static void PPSAngleDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct PPSAnglePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct PPSRatePacket_s));
-+
-+ /* we dont use position controller onboard */
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
-+
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ /* we use position angle controller onboard */
-+ setpoint->mode.roll = modeAbs;
-+ setpoint->mode.pitch = modeAbs;
-+ setpoint->mode.yaw = modeAbs;
-+
-+ setpoint->attitude.roll = values->roll;
-+ setpoint->attitude.pitch = values->pitch;
-+ setpoint->attitude.yaw = values->yaw;
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
-+
-+ /* update cmds */
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ setpoint->pps_mode = modeAngle;
-+}
-+
- /* ---===== 3 - packetDecoders array =====--- */
- const static packetDecoder_t packetDecoders[] = {
- [stopType] = stopDecoder,
-@@ -300,6 +451,9 @@ const static packetDecoder_t packetDecoders[] = {
- [cppmEmuType] = cppmEmuDecoder,
- [altHoldType] = altHoldDecoder,
- [hoverType] = hoverDecoder,
-+ [PPSMotorsType] = PPSMotorsDecoder,
-+ [PPSRateType] = PPSRateDecoder,
-+ [PPSAngleType] = PPSAngleDecoder,
- };
-
- /* Decoder switch */
-diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
-index 0e66c0b..79d5f2c 100644
---- a/src/modules/src/power_distribution_stock.c
-+++ b/src/modules/src/power_distribution_stock.c
-@@ -76,18 +76,19 @@ void powerDistribution(const control_t *control)
- #ifdef QUAD_FORMATION_X
- int16_t r = control->roll / 2.0f;
- int16_t p = control->pitch / 2.0f;
-- motorPower.m1 = limitThrust(control->thrust - r + p + control->yaw);
-- motorPower.m2 = limitThrust(control->thrust - r - p - control->yaw);
-- motorPower.m3 = limitThrust(control->thrust + r - p + control->yaw);
-- motorPower.m4 = limitThrust(control->thrust + r + p - control->yaw);
-+ motorPower.m1 = limitThrust(control->cmd1 - r + p + control->yaw);
-+ motorPower.m2 = limitThrust(control->cmd2 - r - p - control->yaw);
-+ motorPower.m3 = limitThrust(control->cmd3 + r - p + control->yaw);
-+ motorPower.m4 = limitThrust(control->cmd4 + r + p - control->yaw);
-+
- #else // QUAD_FORMATION_NORMAL
-- motorPower.m1 = limitThrust(control->thrust + control->pitch +
-+ motorPower.m1 = limitThrust(control->cmd1 + control->pitch +
- control->yaw);
-- motorPower.m2 = limitThrust(control->thrust - control->roll -
-+ motorPower.m2 = limitThrust(control->cmd2 - control->roll -
- control->yaw);
-- motorPower.m3 = limitThrust(control->thrust - control->pitch +
-+ motorPower.m3 = limitThrust(control->cmd3 - control->pitch +
- control->yaw);
-- motorPower.m4 = limitThrust(control->thrust + control->roll -
-+ motorPower.m4 = limitThrust(control->cmd4 + control->roll -
- control->yaw);
- #endif
-
-diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
-index eccc709..6b9fcc3 100644
---- a/src/modules/src/stabilizer.c
-+++ b/src/modules/src/stabilizer.c
-@@ -130,7 +130,6 @@ static void stabilizerTask(void* param)
- commanderGetSetpoint(&setpoint, &state);
-
- sitAwUpdateSetpoint(&setpoint, &sensorData, &state);
--
- stateController(&control, &setpoint, &sensorData, &state, tick);
-
- checkEmergencyStopTimeout();
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2017-06/manifest.json b/crazyflie-firmware/crazyflie-firmware-version-2017-06/manifest.json
deleted file mode 100644
index 58f97a9ac6d1174015a31ee746aab63d97dba97a..0000000000000000000000000000000000000000
--- a/crazyflie-firmware/crazyflie-firmware-version-2017-06/manifest.json
+++ /dev/null
@@ -1,28 +0,0 @@
-{
- "version": 1,
- "subversion": 1,
- "release": "2017.06",
- "files": {
- "cf2-2017.06_withDfallPatch.bin": {
- "platform": "cf2",
- "target": "stm32",
- "type": "fw",
- "release": "2017.06",
- "repository": "crazyflie-firmware"
- },
- "cf2_nrf-2017.05.bin": {
- "platform": "cf2",
- "target": "nrf51",
- "type": "fw",
- "release": "2017.05",
- "repository": "crazyflie2-nrf-firmware"
- },
- "cf1-2017.06.bin": {
- "platform": "cf1",
- "target": "stm32",
- "type": "fw",
- "release": "2017.06",
- "repository": "crazyflie-firmware"
- }
- }
-}
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin b/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin
index 01fe563354fc0d4ecf38feabf6440f162f703ee3..04fddf6eca9a4b1b9cac7f9c1162e74ae2a84f55 100755
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin and b/crazyflie-firmware/crazyflie-firmware-version-2020-02/cf2-2020.02_withDfallPatch.bin differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip
index 5b372f7f3eef8a006ae39c8c994eedd03ade5e98..fea06a8cc08ee07cdeeba670ba7d17ae49667a89 100644
Binary files a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip and b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware-cf2-2020.02_withDfallPatch.zip differ
diff --git a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch
index 776fa60d4faf6946623680e6b87091a96c573656..6b2d1151426995f11f155d2bb8904bc54fe14748 100644
--- a/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch
+++ b/crazyflie-firmware/crazyflie-firmware-version-2020-02/firmware_modifications_for_version_2020-02.patch
@@ -15,7 +15,7 @@ index fe25dc0..fd69bdc 100644
* Reset controller roll, pitch and yaw PID's.
*/
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
-index fc3e378..272cc88 100644
+index fc3e378..c51fdbd 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -159,6 +159,10 @@ typedef struct control_s {
@@ -29,25 +29,10 @@ index fc3e378..272cc88 100644
} control_t;
typedef enum mode_e {
-@@ -167,6 +171,13 @@ typedef enum mode_e {
- modeVelocity
- } stab_mode_t;
-
-+typedef enum dfall_mode_e {
-+ modeDfallDisable = 0,
-+ modeDfallMotors,
-+ modeDfallRate,
-+ modeDfallAngle,
-+} dfall_mode_t;
-+
- typedef struct setpoint_s {
- uint32_t timestamp;
-
-@@ -174,6 +185,11 @@ typedef struct setpoint_s {
+@@ -174,11 +178,17 @@ typedef struct setpoint_s {
attitude_t attitudeRate; // deg/s
quaternion_t attitudeQuaternion;
float thrust;
-+ dfall_mode_t dfall_mode;
+ uint16_t cmd1;
+ uint16_t cmd2;
+ uint16_t cmd3;
@@ -55,6 +40,13 @@ index fc3e378..272cc88 100644
point_t position; // m
velocity_t velocity; // m/s
acc_t acceleration; // m/s^2
+ bool velocity_body; // true if velocity is given in body frame; false if velocity is given in world frame
+
++ bool useDfallAdjustmentsToControllerPID;
++
+ struct {
+ stab_mode_t x;
+ stab_mode_t y;
diff --git a/src/modules/src/attitude_pid_controller.c b/src/modules/src/attitude_pid_controller.c
index b7d707e..bcb02bc 100644
--- a/src/modules/src/attitude_pid_controller.c
@@ -72,25 +64,10 @@ index b7d707e..bcb02bc 100644
{
pidReset(&pidRoll);
diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
-index f4ecb75..4a9047c 100644
+index f4ecb75..673b5cc 100644
--- a/src/modules/src/controller_pid.c
+++ b/src/modules/src/controller_pid.c
-@@ -74,7 +74,13 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
- }
-
- if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
-- positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ // Check if the "dfall_mode" needs the position controller to run
-+ bool posControllIsNeeded_for_dfall_mode = (setpoint->dfall_mode != modeDfallMotors && setpoint->dfall_mode != modeDfallRate && setpoint->dfall_mode != modeDfallAngle);
-+ // Hence run the position controller only if needed
-+ if (posControllIsNeeded_for_dfall_mode)
-+ {
-+ positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
-+ }
- }
-
- if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
-@@ -132,6 +138,9 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
+@@ -132,6 +132,9 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
control->thrust = actuatorThrust;
}
@@ -100,57 +77,57 @@ index f4ecb75..4a9047c 100644
if (control->thrust == 0)
{
control->thrust = 0;
-@@ -150,6 +159,89 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
+@@ -150,6 +153,92 @@ void controllerPid(control_t *control, setpoint_t *setpoint,
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
+ */
+
-+ // This set of "if:then:else" statements is used to specify
-+ // the "control->cmd{1,2,3,4}" depending on the "setpoint->dfall_mode"
-+ // And in all cases perform the check if either:
-+ // >> "control->thrust==0", or
-+ // >> "control->cmd{1,2,3,4}==0"
-+ // where the check performed is appropriate to the case.
-+ // The result of the check is used to set all commands to zero and
-+ // to reset all PIDs, if necessary.
-+
-+ // Default the check to false
-+ bool isZeroThrust = false;
-+
-+ if (setpoint->dfall_mode == modeDfallMotors)
++ // The following "if:then:else" statements is used to
++ // specify the "control->cmd{1,2,3,4}" depending on
++ // the boolean:
++ // >> "setpoint->useDfallAdjustmentsToControllerPID"
++ if (setpoint->useDfallAdjustmentsToControllerPID)
+ {
+ // Set the {roll,pitch,yaw} commands to zero
-+ control->roll = 0;
-+ control->pitch = 0;
-+ control->yaw = 0;
-+ // Copy across the per motor commands
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ // Perform check for all thrust being zero
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
++ // if indicated by the respective mode
++ if (setpoint->mode.roll == modeDisable)
+ {
-+ // Set the flag to true that thrusts are zero
-+ isZeroThrust = true;
++ control->roll = 0;
++ cmd_roll = 0.0;
+ }
-+ }
-+ else if(setpoint->dfall_mode == modeDfallRate || setpoint->dfall_mode == modeDfallAngle)
-+ {
-+ // Copy across the per motor commands
-+ control->cmd1 = setpoint->cmd1;
-+ control->cmd2 = setpoint->cmd2;
-+ control->cmd3 = setpoint->cmd3;
-+ control->cmd4 = setpoint->cmd4;
-+ // Perform check for all thrust being zero
-+ if ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0))
++ if (setpoint->mode.pitch == modeDisable)
++ {
++ control->pitch = 0;
++ cmd_pitch = 0.0;
++ }
++ if (setpoint->mode.yaw == modeDisable)
++ {
++ control->yaw = 0;
++ cmd_yaw = 0.0;
++ }
++ // Apply the per-motor commands differently
++ // depending on the Z-controller mode
++ if (setpoint->mode.z == modeDisable)
+ {
-+ // Set the flag to true that thrusts are zero
-+ isZeroThrust = true;
++ // Copy across the per motor commands
++ control->cmd1 = setpoint->cmd1;
++ control->cmd2 = setpoint->cmd2;
++ control->cmd3 = setpoint->cmd3;
++ control->cmd4 = setpoint->cmd4;
++ }
++ else
++ {
++ // Add the setpoints on top of the
++ // "control thrust" computed by the
++ // PID controllers above
++ control->cmd1 = setpoint->cmd1 + control->thrust;
++ control->cmd2 = setpoint->cmd2 + control->thrust;
++ control->cmd3 = setpoint->cmd3 + control->thrust;
++ control->cmd4 = setpoint->cmd4 + control->thrust;
+ }
+ }
-+ // Otherwise, "setpoint->dfall_mode == modeDfallDisable"
++ // Otherwise, "setpoint->useDfallAdjustmentsToControllerPID == false"
+ else
+ {
+ // Put the thrust value onto each motor
@@ -158,16 +135,13 @@ index f4ecb75..4a9047c 100644
+ control->cmd2 = control->thrust;
+ control->cmd3 = control->thrust;
+ control->cmd4 = control->thrust;
-+ // If thrust is zero, then:
-+ if (control->thrust == 0)
-+ {
-+ // Set the flag to true that thrusts are zero
-+ isZeroThrust = true;
-+ }
+ }
+
-+ // If the flag idicates that thrusts are zero,
-+ // then set all commands to zero and reset the PIDs
++ // If all control commands are zero, i.e., if:
++ // >> "control->cmd{1,2,3,4}==0"
++ // then set all commands to zero and to reset
++ // all PIDs.
++ bool isZeroThrust = ((control->cmd1 == 0 && control->cmd2 == 0 && control->cmd3 == 0 && control->cmd4 == 0));
+ if (isZeroThrust)
+ {
+ // Set all commands to zero
@@ -186,217 +160,412 @@ index f4ecb75..4a9047c 100644
+
+ // Reset the calculated YAW angle for rate control
+ attitudeDesired.yaw = state->attitude.yaw;
++
++ // Update also the log variables to reflect this
++ cmd_thrust = 0.0;
++ cmd_roll = 0.0;
++ cmd_pitch = 0.0;
++ cmd_yaw = 0.0;
+ }
}
diff --git a/src/modules/src/crtp_commander_generic.c b/src/modules/src/crtp_commander_generic.c
-index 1662fe4..84d0631 100644
+index 1662fe4..f6931a4 100644
--- a/src/modules/src/crtp_commander_generic.c
+++ b/src/modules/src/crtp_commander_generic.c
-@@ -71,6 +71,9 @@ enum packet_type {
+@@ -71,6 +71,7 @@ enum packet_type {
hoverType = 5,
fullStateType = 6,
positionType = 7,
-+ dfallMotorsType = 8,
-+ dfallRateType = 9,
-+ dfallAngleType = 10,
++ dfallType = 8,
};
/* ---===== 2 - Decoding functions =====--- */
-@@ -367,6 +370,180 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
- setpoint->attitude.yaw = values->yaw;
+@@ -112,6 +113,9 @@ static void velocityDecoder(setpoint_t *setpoint, uint8_t type, const void *data
+ setpoint->mode.yaw = modeVelocity;
+
+ setpoint->attitudeRate.yaw = -values->yawrate;
++
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
}
-+/* dfall motors mode decoder
-+ * disable all controllers, the motor commands provided are
-+ * set directly to the motors
-+ */
-+struct dfallMotorsPacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+} __attribute__((packed));
+ /* zDistanceDecoder
+@@ -145,6 +149,9 @@ static void zDistanceDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
+
+ setpoint->attitude.roll = values->roll;
+ setpoint->attitude.pitch = values->pitch;
+
-+static void dfallMotorsDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct dfallMotorsPacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct dfallMotorsPacket_s));
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* cppmEmuDecoder
+@@ -230,6 +237,9 @@ static void cppmEmuDecoder(setpoint_t *setpoint, uint8_t type, const void *data,
+ {
+ setpoint->thrust = 0;
+ }
+
-+ // Disable the onboard position controllers
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* altHoldDecoder
+@@ -263,6 +273,9 @@ static void altHoldDecoder(setpoint_t *setpoint, uint8_t type, const void *data,
+
+ setpoint->attitude.roll = values->roll;
+ setpoint->attitude.pitch = values->pitch;
+
-+ // And set their setpoint values to zero
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ // Disable the onboard orientation controllers
-+ setpoint->mode.roll = modeDisable;
-+ setpoint->mode.pitch = modeDisable;
-+ setpoint->mode.yaw = modeDisable;
-+
-+ // And set their setpoint values to zero
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* hoverDecoder
+@@ -294,6 +307,9 @@ static void hoverDecoder(setpoint_t *setpoint, uint8_t type, const void *data, s
+ setpoint->velocity.y = values->vy;
+
+ setpoint->velocity_body = true;
+
-+ // Tranfer the per-motor commands from the packet
-+ // to the respective setpoints
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ struct fullStatePacket_s {
+@@ -338,6 +354,9 @@ static void fullStateDecoder(setpoint_t *setpoint, uint8_t type, const void *dat
+ setpoint->mode.roll = modeDisable;
+ setpoint->mode.pitch = modeDisable;
+ setpoint->mode.yaw = modeDisable;
++
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+ }
+
+ /* positionDecoder
+@@ -365,6 +384,313 @@ static void positionDecoder(setpoint_t *setpoint, uint8_t type, const void *data
+ setpoint->mode.yaw = modeAbs;
+
+ setpoint->attitude.yaw = values->yaw;
+
-+ // Set the "dfall_mode" accordingly
-+ setpoint->dfall_mode = modeDfallMotors;
++ // Turn off the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = false;
+}
+
-+/* dfall rate mode decoder
-+ * disable the position controllers and use the attitude controllers
-+ * in "velocity mode", i.e., use the onboard "rate controllers"
++/* dfall mode decoder
++ * set the mode of the onboard {x,y,z,roll,pitch,yaw}
++ * controllers based on the mode specification
++ * contained in the packet. A separate mode is
++ * specified for each of the four independent
++ * coordinates of {x,y,z,yaw}
+ */
-+struct dfallRatePacket_s {
-+ uint16_t cmd1;
-+ uint16_t cmd2;
-+ uint16_t cmd3;
-+ uint16_t cmd4;
-+ float roll_rate;
-+ float pitch_rate;
-+ float yaw_rate;
-+} __attribute__((packed));
-+
-+static void dfallRateDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
-+{
-+ const struct dfallRatePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct dfallRatePacket_s));
-+
-+ // Disable the onboard position controllers
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
+
-+ // And set their setpoint values to zero
-+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ // Enable the onboard orientation controllers in
-+ // velocity mode, i.e., as rate controllers
-+ setpoint->mode.roll = modeVelocity;
-+ setpoint->mode.pitch = modeVelocity;
-+ setpoint->mode.yaw = modeVelocity;
-+
-+ // Set the angle setpoints to zero
-+ setpoint->attitude.roll = 0;
-+ setpoint->attitude.pitch = 0;
-+ setpoint->attitude.yaw = 0;
-+ // Transfer the angular rate commands from the packet
-+ // to the respective setpoints
-+ setpoint->attitudeRate.roll = values->roll_rate;
-+ setpoint->attitudeRate.pitch = values->pitch_rate;
-+ setpoint->attitudeRate.yaw = values->yaw_rate;
++#define DFALL_CONTROLLER_MODE_OFF 0
++#define DFALL_CONTROLLER_MODE_ANGULAR_RATE 1
++#define DFALL_CONTROLLER_MODE_ANGLE 2
++#define DFALL_CONTROLLER_MODE_VELOCITY 3
++#define DFALL_CONTROLLER_MODE_POSITION 4
+
-+ // Tranfer the per-motor commands from the packet
-+ // to the respective setpoints
-+ setpoint->cmd1 = limitUint16(values->cmd1);
-+ setpoint->cmd2 = limitUint16(values->cmd2);
-+ setpoint->cmd3 = limitUint16(values->cmd3);
-+ setpoint->cmd4 = limitUint16(values->cmd4);
-+
-+ // Set the "dfall_mode" accordingly
-+ setpoint->dfall_mode = modeDfallRate;
-+}
-+
-+/* dfall angle mode decoder
-+ * disable the position controllers and use the attitude controllers
-+ * in "absolute mode", i.e., use the onboard "angle controllers"
-+ */
-+struct dfallAnglePacket_s {
++struct dfallPacket_s {
++ uint16_t controllerModes;
+ uint16_t cmd1;
+ uint16_t cmd2;
+ uint16_t cmd3;
+ uint16_t cmd4;
-+ float roll;
-+ float pitch;
-+ float yaw;
++ float xControllerSetpoint;
++ float yControllerSetpoint;
++ float zControllerSetpoint;
++ float yawControllerSetpoint;
+} __attribute__((packed));
+
-+static void dfallAngleDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
++static void dfallDecoder(setpoint_t *setpoint, uint8_t type, const void *data, size_t datalen)
+{
-+ const struct dfallAnglePacket_s *values = data;
-+ ASSERT(datalen == sizeof(struct dfallRatePacket_s));
++ const struct dfallPacket_s *values = data;
++ ASSERT(datalen == sizeof(struct dfallPacket_s));
++
++ // DECODE THE 16-BIT INTEGER THAT ENCODES
++ // THE REQUESTED {x,y,z,yaw} CONTROLLER MODES
++ // > Each mode is encoded using 3 bits, stacked together as follows:
++ // ---------------------------------------------------------------
++ // |BIT | 15 14 13 12 | 11 10 9 | 8 7 6 | 5 4 3 | 2 1 0 |
++ // |DESC. | free bits | yaw_mode | z_mode | y_mode | x_mode |
++ // ---------------------------------------------------------------
++
++ uint16_t mode_selector_bits = 7;
++
++ // EXTRACT THE MODE FOR EACH CONTROLLER
++ uint8_t x_controller_mode = (values->controllerModes >> 0) & mode_selector_bits;
++ uint8_t y_controller_mode = (values->controllerModes >> 3) & mode_selector_bits;
++ uint8_t z_controller_mode = (values->controllerModes >> 6) & mode_selector_bits;
++ uint8_t yaw_controller_mode = (values->controllerModes >> 9) & mode_selector_bits;
++
++ // INITIALISE A BOOLEAN FOR TRACKING IF ANY
++ // OF THE CONTROLLER MODES ARE INVALID
++ bool invalid_controller_mode_requested = false;
++
++ // PROCESS THE X CONTROLLER
++ switch (x_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeVelocity;
++ // Set the setpoint
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = values->xControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeAbs;
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = values->xControllerSetpoint;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ // Set the modes
++ setpoint->mode.x = modeVelocity;
++ setpoint->mode.pitch = modeAbs;
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = values->xControllerSetpoint;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_POSITION:
++ // Set the modes
++ setpoint->mode.x = modeAbs;
++ setpoint->mode.pitch = modeAbs;
++ // Set the setpoints
++ setpoint->position.x = values->xControllerSetpoint;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeDisable;
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ break;
++
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
++
++ // PROCESS THE Y CONTROLLER
++ switch (y_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ // Set the modes
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeVelocity;
++ // Set the setpoint
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = values->yControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ // Set the modes
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeAbs;
++ // Set the setpoints
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = values->yControllerSetpoint;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ // Set the modes
++ setpoint->mode.y = modeVelocity;
++ setpoint->mode.roll = modeAbs;
++ // Set the setpoints
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = values->yControllerSetpoint;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_POSITION:
++ // Set the modes
++ setpoint->mode.y = modeAbs;
++ setpoint->mode.roll = modeAbs;
++ // Set the setpoints
++ setpoint->position.y = values->yControllerSetpoint;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the modes
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeDisable;
++ // Set the setpoints
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
++
++ // PROCESS THE Z CONTROLLER
++ switch (z_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ // Set the mode
++ setpoint->mode.z = modeVelocity;
++ // Set the setpoints
++ setpoint->position.z = 0.0;
++ setpoint->velocity.z = values->zControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_POSITION:
++ // Set the mode
++ setpoint->mode.z = modeAbs;
++ // Set the setpoints
++ setpoint->position.z = values->zControllerSetpoint;
++ setpoint->velocity.z = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the mode
++ setpoint->mode.z = modeDisable;
++ // Set the setpoints
++ setpoint->position.z = 0.0;
++ setpoint->velocity.z = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
+
-+ // Disable the onboard position controllers
-+ setpoint->mode.x = modeDisable;
-+ setpoint->mode.y = modeDisable;
-+ setpoint->mode.z = modeDisable;
++ // PROCESS THE YAW CONTROLLER
++ switch (yaw_controller_mode)
++ {
++ case DFALL_CONTROLLER_MODE_ANGULAR_RATE:
++ // Set the mode
++ setpoint->mode.yaw = modeVelocity;
++ // Set the setpoint
++ setpoint->attitude.yaw = 0.0;
++ setpoint->attitudeRate.yaw = values->yawControllerSetpoint;
++ break;
++
++ case DFALL_CONTROLLER_MODE_ANGLE:
++ // Set the modes
++ setpoint->mode.yaw = modeAbs;
++ // Set the setpoints
++ setpoint->attitude.roll = values->yawControllerSetpoint;
++ setpoint->attitudeRate.roll = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_OFF:
++ // Set the modes
++ setpoint->mode.yaw = modeDisable;
++ // Set the setpoints
++ setpoint->attitude.yaw = 0.0;
++ setpoint->attitudeRate.yaw = 0.0;
++ break;
++
++ case DFALL_CONTROLLER_MODE_VELOCITY:
++ case DFALL_CONTROLLER_MODE_POSITION:
++ default:
++ // Set the flag that an invalid mode
++ // was requested
++ invalid_controller_mode_requested = true;
++ break;
++ }
+
-+ // And set their setpoint values to zero
++ // Set the thrust setpoint to zero
+ setpoint->thrust = 0;
-+ setpoint->position.x = 0;
-+ setpoint->position.y = 0;
-+ setpoint->position.z = 0;
-+ setpoint->velocity.x = 0;
-+ setpoint->velocity.y = 0;
-+ setpoint->velocity.z = 0;
-+
-+ // Enable the onboard orientation controllers in
-+ // absolute mode, i.e., as angle controllers
-+ setpoint->mode.roll = modeAbs;
-+ setpoint->mode.pitch = modeAbs;
-+ setpoint->mode.yaw = modeAbs;
-+
-+ // Transfer the angle commands from the packet
-+ // to the respective setpoints
-+ setpoint->attitude.roll = values->roll;
-+ setpoint->attitude.pitch = values->pitch;
-+ setpoint->attitude.yaw = values->yaw;
-+ // Set the angular rate setpoints to zero
-+ setpoint->attitudeRate.roll = 0;
-+ setpoint->attitudeRate.pitch = 0;
-+ setpoint->attitudeRate.yaw = 0;
+
+ // Tranfer the per-motor commands from the packet
+ // to the respective setpoints
++ // NOTES:
++ // (1) this is performed regardless of the
++ // "z_controller_mode" because the per motor
++ // commands can be used for correcting steady-
++ // state offsets in {x,y,yaw}.
++ // (2) if the "z_controller_mode" is in "POSITION"
++ // or "VELOCITY" mode, then onboard controllers
++ // will compute the main thurst controler, and
++ // the user is expected to fill in cmd{1,2,3,4}
++ // with only adjustment values.
+ setpoint->cmd1 = limitUint16(values->cmd1);
+ setpoint->cmd2 = limitUint16(values->cmd2);
+ setpoint->cmd3 = limitUint16(values->cmd3);
+ setpoint->cmd4 = limitUint16(values->cmd4);
+
-+ // Set the "dfall_mode" accordingly
-+ setpoint->dfall_mode = modeDfallAngle;
-+}
++ // Set everything to off and zero if an invlaid
++ // controller mode was requested
++ if (invalid_controller_mode_requested)
++ {
++ // Set the modes
++ setpoint->mode.x = modeDisable;
++ setpoint->mode.pitch = modeDisable;
++ setpoint->mode.y = modeDisable;
++ setpoint->mode.roll = modeDisable;
++ setpoint->mode.z = modeDisable;
++ setpoint->mode.yaw = modeDisable;
++
++ // Set the setpoints
++ setpoint->position.x = 0.0;
++ setpoint->velocity.x = 0.0;
++ setpoint->attitude.pitch = 0.0;
++ setpoint->attitudeRate.pitch = 0.0;
++ setpoint->position.y = 0.0;
++ setpoint->velocity.y = 0.0;
++ setpoint->attitude.roll = 0.0;
++ setpoint->attitudeRate.roll = 0.0;
++ setpoint->position.z = 0.0;
++ setpoint->velocity.z = 0.0;
++ setpoint->attitude.yaw = 0.0;
++ setpoint->attitudeRate.yaw = 0.0;
++ }
+
++ // Turn on the "dfall mode adjustments"
++ setpoint->useDfallAdjustmentsToControllerPID = true;
+ }
+
/* ---===== 3 - packetDecoders array =====--- */
- const static packetDecoder_t packetDecoders[] = {
- [stopType] = stopDecoder,
-@@ -377,6 +554,9 @@ const static packetDecoder_t packetDecoders[] = {
+@@ -377,6 +703,7 @@ const static packetDecoder_t packetDecoders[] = {
[hoverType] = hoverDecoder,
[fullStateType] = fullStateDecoder,
[positionType] = positionDecoder,
-+ [dfallMotorsType] = dfallMotorsDecoder,
-+ [dfallRateType] = dfallRateDecoder,
-+ [dfallAngleType] = dfallAngleDecoder,
++ [dfallType] = dfallDecoder,
};
/* Decoder switch */
diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
-index d3b7d8d..4e7f311 100644
+index d3b7d8d..072a699 100644
--- a/src/modules/src/power_distribution_stock.c
+++ b/src/modules/src/power_distribution_stock.c
@@ -80,18 +80,18 @@ void powerDistribution(const control_t *control)
@@ -435,13 +604,10 @@ index d3b7d8d..4e7f311 100644
LOG_GROUP_START(motor)
-LOG_ADD(LOG_INT32, m4, &motorPower.m4)
--LOG_ADD(LOG_INT32, m1, &motorPower.m1)
--LOG_ADD(LOG_INT32, m2, &motorPower.m2)
--LOG_ADD(LOG_INT32, m3, &motorPower.m3)
-+LOG_ADD(LOG_UINT32, m1, &motorPower.m1)
-+LOG_ADD(LOG_UINT32, m2, &motorPower.m2)
-+LOG_ADD(LOG_UINT32, m3, &motorPower.m3)
-+LOG_ADD(LOG_UINT32, m4, &motorPower.m4)
+ LOG_ADD(LOG_INT32, m1, &motorPower.m1)
+ LOG_ADD(LOG_INT32, m2, &motorPower.m2)
+ LOG_ADD(LOG_INT32, m3, &motorPower.m3)
++LOG_ADD(LOG_INT32, m4, &motorPower.m4)
LOG_GROUP_STOP(motor)
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 372ec94..cb4f848 100644
diff --git a/crazyflie-firmware/success_building_for_version_2017-06.png b/crazyflie-firmware/success_building_for_version_2017-06.png
deleted file mode 100644
index f48fca5634c490355d66642170d49b5fe537af3f..0000000000000000000000000000000000000000
Binary files a/crazyflie-firmware/success_building_for_version_2017-06.png and /dev/null differ
diff --git a/crazyflie-firmware/success_building_for_version_2020-02.png b/crazyflie-firmware/success_building_for_version_2020-02.png
index 854299c68d6d3d49277d711d0e8608732f943949..62c3e54314a0fd24a7fbe4597521d22146530d38 100644
Binary files a/crazyflie-firmware/success_building_for_version_2020-02.png and b/crazyflie-firmware/success_building_for_version_2020-02.png differ
diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index 292aec970604a3f4086a8bdc1fec47eb5c0d6664..1c4d9be33e6baf59676aa28724071a289fca1df0 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/dfall_ws/src/dfall_pkg/CMakeLists.txt
@@ -249,16 +249,20 @@ endif()
add_message_files(
FILES
- UnlabeledMarker.msg
- CrazyflieData.msg
- ViconData.msg
+ AreaBounds.msg
ControlCommand.msg
CrazyflieContext.msg
- AreaBounds.msg
+ CrazyflieDB.msg
+ CrazyflieEntry.msg
+ CustomButton.msg
+ DebugMsg.msg
+ FlyingVehicleState.msg
+ GyroMeasurements.msg
Setpoint.msg
SetpointV2.msg
- CrazyflieEntry.msg
- CrazyflieDB.msg
+ UnlabeledMarker.msg
+ ViconData.msg
+ ViconSubscribeObjectName.msg
#------------------------
IntWithHeader.msg
FloatWithHeader.msg
@@ -266,9 +270,6 @@ add_message_files(
SetpointWithHeader.msg
CustomButtonWithHeader.msg
#------------------------
- DebugMsg.msg
- CustomButton.msg
- ViconSubscribeObjectName.msg
)
## Generate services in the 'srv' folder
@@ -280,6 +281,7 @@ add_message_files(
add_service_files(
FILES
IntIntService.srv
+ IntStringService.srv
Controller.srv
CMRead.srv
CMQuery.srv
@@ -287,6 +289,7 @@ add_service_files(
CMUpdate.srv
CMCommand.srv
LoadYamlFromFilename.srv
+ GetDebugValuesService.srv
GetSetpointService.srv
)
@@ -391,41 +394,43 @@ endif()
# add_executable(${PROJECT_NAME}_node src/dfall_pkg_node.cpp)
if(VICON_LIBRARY)
- add_executable(ViconDataPublisher src/nodes/ViconDataPublisher.cpp)
+ add_executable(ViconDataPublisher src/nodes/ViconDataPublisher.cpp)
endif()
-add_executable(FlyingAgentClient src/nodes/FlyingAgentClient.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(CrazyRadioEmulator src/nodes/CrazyRadioEmulator.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(CsoneControllerService src/nodes/CsoneControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(DefaultControllerService src/nodes/DefaultControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
-add_executable(DemoControllerService src/nodes/DemoControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(StudentControllerService src/nodes/StudentControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
-add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(TuningControllerService src/nodes/TuningControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(PickerControllerService src/nodes/PickerControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
-add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(FlyingAgentClient src/nodes/FlyingAgentClient.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(AgentStatusForWebInterface src/nodes/AgentStatusForWebInterface.cpp)
+add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CrazyRadioEmulator src/nodes/CrazyRadioEmulator.cpp
+ src/classes/QuadrotorSimulator.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CsoneControllerService src/nodes/CsoneControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(DefaultControllerService src/nodes/DefaultControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+# add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
+# add_executable(DemoControllerService src/nodes/DemoControllerService.cpp
+# src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(StudentControllerService src/nodes/StudentControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+# add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
+add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(TuningControllerService src/nodes/TuningControllerService.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
+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)
+ src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/classes/CrazyflieIO.cpp)
+add_executable(ParameterService src/nodes/ParameterService.cpp)
-add_executable(MocapEmulator src/nodes/MocapEmulator.cpp
- src/classes/QuadrotorSimulator.cpp
- src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(MocapEmulator src/nodes/MocapEmulator.cpp
+ src/classes/QuadrotorSimulator.cpp
+ src/classes/GetParamtersAndNamespaces.cpp)
@@ -493,24 +498,25 @@ if(VICON_LIBRARY)
add_dependencies(ViconDataPublisher dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
endif()
-add_dependencies(FlyingAgentClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(BatteryMonitor dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(CrazyRadioEmulator dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(CsoneControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(DefaultControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(SafeControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(DemoControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(StudentControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(MpcControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-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(FlyingAgentClient dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(AgentStatusForWebInterface dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(BatteryMonitor dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CrazyRadioEmulator dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CsoneControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(DefaultControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+# add_dependencies(SafeControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+# add_dependencies(DemoControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(StudentControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+# add_dependencies(MpcControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(RemoteControllerService dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+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})
+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})
+add_dependencies(MocapEmulator dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -547,28 +553,29 @@ if(VICON_LIBRARY)
target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
endif()
-target_link_libraries(FlyingAgentClient ${catkin_LIBRARIES})
-target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
-target_link_libraries(CrazyRadioEmulator ${catkin_LIBRARIES})
-target_link_libraries(CsoneControllerService ${catkin_LIBRARIES})
-target_link_libraries(DefaultControllerService ${catkin_LIBRARIES})
-target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
-target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
-target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
-if(Eigen3_FOUND)
- target_link_libraries(MpcControllerService ${catkin_LIBRARIES} Eigen3::Eigen)
-else()
- target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
-endif()
-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(FlyingAgentClient ${catkin_LIBRARIES})
+target_link_libraries(AgentStatusForWebInterface ${catkin_LIBRARIES})
+target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
+target_link_libraries(CrazyRadioEmulator ${catkin_LIBRARIES})
+target_link_libraries(CsoneControllerService ${catkin_LIBRARIES})
+target_link_libraries(DefaultControllerService ${catkin_LIBRARIES})
+# target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
+# target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
+target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
+# if(Eigen3_FOUND)
+# target_link_libraries(MpcControllerService ${catkin_LIBRARIES} Eigen3::Eigen)
+# else()
+# target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
+# endif()
+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})
+target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
+target_link_libraries(ParameterService ${catkin_LIBRARIES})
-target_link_libraries(MocapEmulator ${catkin_LIBRARIES})
+target_link_libraries(MocapEmulator ${catkin_LIBRARIES})
if(Qt5_FOUND)
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
index d92fc08b2c7ba41f048c17b9d4c7bcdee4802863..9a431b5b4854d394344fa58de136469fde74bfae 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/enablecontrollerloadyamlbar.ui
@@ -6,7 +6,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1572</width>
+ <width>1784</width>
<height>403</height>
</rect>
</property>
@@ -30,87 +30,6 @@
<property name="spacing">
<number>12</number>
</property>
- <item row="1" column="5">
- <widget class="QPushButton" name="load_yaml_tuning_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Tuning</string>
- </property>
- </widget>
- </item>
- <item row="1" column="7">
- <widget class="QPushButton" name="load_yaml_template_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Template</string>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QPushButton" name="enable_student_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Student</string>
- </property>
- </widget>
- </item>
<item row="0" column="0">
<widget class="QLabel" name="enable_controller_label">
<property name="sizePolicy">
@@ -145,149 +64,6 @@
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QPushButton" name="load_yaml_student_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Student</string>
- </property>
- </widget>
- </item>
- <item row="0" column="5">
- <widget class="QPushButton" name="enable_tuning_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Tuning</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QPushButton" name="enable_default_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QPushButton" name="enable_picker_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Picker</string>
- </property>
- </widget>
- </item>
- <item row="0" column="7">
- <widget class="QPushButton" name="enable_template_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="font">
- <font>
- <weight>50</weight>
- <bold>false</bold>
- </font>
- </property>
- <property name="text">
- <string>Template</string>
- </property>
- </widget>
- </item>
<item row="1" column="0">
<widget class="QLabel" name="load_yaml_label">
<property name="sizePolicy">
@@ -322,167 +98,440 @@
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QPushButton" name="load_yaml_default_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="0" column="8">
- <spacer name="horizontalSpacer">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>40</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="0" column="6">
- <widget class="QPushButton" name="enable_remote_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Remote</string>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QPushButton" name="load_yaml_remote_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Remote</string>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QPushButton" name="load_yaml_picker_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>Picker</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="enable_csone_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>49</height>
- </size>
- </property>
- <property name="text">
- <string>CS1</string>
- </property>
+ <item row="1" column="2">
+ <widget class="QWidget" name="widget_yamlButtons" native="true">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QPushButton" name="load_yaml_default_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_student_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Student</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_csone_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_picker_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Picker</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_tuning_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Tuning</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_remote_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="load_yaml_template_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Template</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</widget>
</item>
- <item row="1" column="3">
- <widget class="QPushButton" name="load_yaml_csone_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>60</width>
- <height>50</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>50</height>
- </size>
- </property>
- <property name="text">
- <string>CS1</string>
- </property>
+ <item row="0" column="2">
+ <widget class="QWidget" name="widget_enableButtons" native="true">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QPushButton" name="enable_default_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_student_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Student</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_csone_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>49</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>CS1</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_picker_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Picker</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_tuning_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Tuning</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_remote_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="enable_template_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>60</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <weight>50</weight>
+ <bold>false</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Template</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</widget>
</item>
</layout>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
index aa37362f82fe3ccd8e170ddccc5d29abb8e9f4e9..401d1042c7eac0c7f10b21c1d6f66b94ba5d6840 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/mainwindow.ui
@@ -190,14 +190,14 @@
<x>0</x>
<y>0</y>
<width>1862</width>
- <height>26</height>
+ <height>47</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
<property name="title">
<string>File</string>
</property>
- <addaction name="actionShowHide_Coordinator"/>
+ <addaction name="action_showHide_Coordinator"/>
</widget>
<widget class="QMenu" name="menuLoad_YAML">
<property name="title">
@@ -205,6 +205,8 @@
</property>
<addaction name="action_LoadYAML_BatteryMonitor"/>
<addaction name="action_LoadYAML_FlyingAgentClientConfig"/>
+ <addaction name="separator"/>
+ <addaction name="action_showHide_loadYamlBar"/>
</widget>
<widget class="QMenu" name="menuControllers">
<property name="title">
@@ -233,7 +235,7 @@
</attribute>
</widget>
<widget class="QStatusBar" name="statusBar"/>
- <action name="actionShowHide_Coordinator">
+ <action name="action_showHide_Coordinator">
<property name="text">
<string>Hide Coordinator</string>
</property>
@@ -344,6 +346,11 @@
<string>CS1</string>
</property>
</action>
+ <action name="action_showHide_loadYamlBar">
+ <property name="text">
+ <string>Hide Yaml Buttons</string>
+ </property>
+ </action>
</widget>
<layoutdefault spacing="6" margin="11"/>
<customwidgets>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
index 08df7f2abf692151d88cdb129af0466fc6f39d29..d7bff72dee05aa975312b82d7a32184ac15c1db4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/controllertabs.h
@@ -57,7 +57,7 @@
// Include the DFALL message types
//#include "dfall_pkg/IntWithHeader.h"
//#include "dfall_pkg/SetpointWithHeader.h"
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/AreaBounds.h"
#include "dfall_pkg/CrazyflieContext.h"
@@ -108,7 +108,7 @@ public slots:
signals:
void agentIDsToCoordinateChanged(QVector<int> agentIDs , bool shouldCoordinateAll);
- void measuredPoseValueChanged(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void measuredPoseValueChanged(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSignal();
@@ -146,6 +146,12 @@ private:
// Sound effect for when the controller changes while flying
//QSoundEffect m_soundEffect_controllerChanged;
+ // FLAG FOR WHAT DATA TO EMIT
+ // > For "poseDataMocap"
+ bool m_shouldEmitPoseDataMocap = true;
+ // > For "poseDataOnboard"
+ bool m_shouldEmitPoseDataOnboard = false;
+
#ifdef CATKIN_MAKE
// --------------------------------------------------- //
@@ -157,7 +163,9 @@ private:
// SUBSRIBER
// > For the pose data from a motion capture system
- ros::Subscriber m_poseDataSubscriber;
+ ros::Subscriber m_poseDataMocapSubscriber;
+ // > For the pose data from a onboard state estimate
+ ros::Subscriber m_poseDataOnboardSubscriber;
// > For the controller that is currently operating
ros::Subscriber controllerUsedSubscriber;
@@ -171,10 +179,16 @@ private:
// --------------------------------------------------- //
// PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES
+ // > For the data from the motion capture system, received on
+ // "m_poseDataMocapSubscriber"
+ void poseDataMocapReceivedCallback(const dfall_pkg::ViconData& viconData);
+
+ // > For the data from onboard the flying agent, received on
+ // "m_poseDataOnboardSubscriber"
+ void poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehicleState& vehicleState);
+
// > For the controller currently operating, received on
// "controllerUsedSubscriber"
- void poseDataReceivedCallback(const dfall_pkg::ViconData& viconData);
-
void controllerUsedChangedCallback(const std_msgs::Int32& msg);
// Get the paramters that specify the type and ID
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
index 5ba4e0551fd52aba071661895065dfd3b6e7d102..9bdef6b6c2c666f61d2e2ddc281977cb7ae477fc 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
@@ -114,7 +114,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
index 64f315c4f1710841b4c8bdb502a913f57fad39ab..c462a1b191142094733eb7a569260bc6f5b14459 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/defaultcontrollertab.h
@@ -89,7 +89,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
index c411e2d12fc07b1844615f6ed68c843a239e9aa0..888f6cad5dd905d34adfdc0eb07304ae6604ff6c 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/enablecontrollerloadyamlbar.h
@@ -95,9 +95,15 @@ public:
void showHideController_remote_changed();
void showHideController_template_changed();
void showHideController_csone_changed();
+
+ // PUBLIC METHOD FOR TOGGLING THE VISIBILITY OF THE YAML BAR
+ bool showHide_loadYamlBar_triggered();
+
+ // PUBLIC METHODS FOR ENABLING THE TEST MOTORS CONTROLLER
void testMotors_triggered();
+
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
index 09cab55768363e2a94de5314c39ba75f9642b009..63a0e37630ce6f13d0a847caa0a9df5c8ffbf75d 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/mainwindow.h
@@ -122,7 +122,8 @@ private:
private slots:
// PRIVATE METHODS FOR MENU ITEM CALLBACKS
- void on_actionShowHide_Coordinator_triggered();
+ void on_action_showHide_Coordinator_triggered();
+ void on_action_showHide_loadYamlBar_triggered();
void on_action_LoadYAML_BatteryMonitor_triggered();
void on_action_LoadYAML_FlyingAgentClientConfig_triggered();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
index 52b79e13875c744e4351bcca11989c6a293cc87a..12270b725c154858df5857833257404ac045b3ec 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
@@ -117,7 +117,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
index 60d50bc203614b14d18b457e91d3acabe21704dc..1738a169395937dbe053b61ad41fdad9fb21c2dc 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/remotecontrollertab.h
@@ -56,7 +56,7 @@
//#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/ViconSubscribeObjectName.h"
// Include the DFALL service types
@@ -94,7 +94,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
@@ -176,8 +176,8 @@ private:
// COPIED FROM PREVIOUS REMOTE CONTROLLER GUI
// For receiving message with the data of the remote
- void remoteDataCallback(const dfall_pkg::CrazyflieData& objectData);
- void remoteControlSetpointCallback(const dfall_pkg::CrazyflieData& setpointData);
+ void remoteDataCallback(const dfall_pkg::FlyingVehicleState& objectData);
+ void remoteControlSetpointCallback(const dfall_pkg::FlyingVehicleState& setpointData);
// For receiving message that the setpoint was changed
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
index 79c447e84f9131164b1faaef335d1d89c06e7ea4..bd16b6041d939a07934b0e5b42d59102741d7de2 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/rosNodeThread_for_flyingAgentGUI.h
@@ -52,7 +52,7 @@
#include "dfall_pkg/CMCommand.h"
//#include "dfall_pkg/UnlabeledMarker.h"
-//#include "dfall_pkg/CrazyflieData.h"
+//#include "dfall_pkg/FlyingVehicleState.h"
//#include "dfall_pkg/ViconData.h"
using namespace dfall_pkg;
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
index be7ed5f428526feac37a3651024fd53c41c22a30..587290b0b9c2a117d792c8ff3129baa18f9f8a35 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/studentcontrollertab.h
@@ -90,7 +90,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h
index 0f9377bb2fa435ca5b98dd792bbde4b863b6fbc2..119c326fe83ea55cd00df9745eeb3aa694e6bff4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/templatecontrollertab.h
@@ -92,7 +92,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
index cf8ecf0525abc397d37426452aef8d3c4a90bba1..d680c50cb4f22eb0f577da818073891af00a496b 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tuningcontrollertab.h
@@ -112,7 +112,7 @@ public:
public slots:
void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
- void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid);
void poseDataUnavailableSlot();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index c29d10d692ea6cf2e3e89f0bf6af1558c509b5b4..ebd02d934a4a4ebc75a16d2bceb273ea1d600354 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
@@ -202,6 +202,28 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
+ // GET THE "onload" PARAMETERS FOR WHAT DATA TO EMIT
+ // > 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("~");
+ // > For "poseDataMocap"
+ if(!nodeHandle.getParam("onlaunch_shouldEmitPoseDataMocap", m_shouldEmitPoseDataMocap))
+ {
+ m_shouldEmitPoseDataMocap = false;
+ ROS_ERROR("[CONTROLLER TABS GUI] missing parameter 'onlaunch_shouldEmitPoseDataMocap'");
+ }
+ // > For "poseDataOnboard"
+ if(!nodeHandle.getParam("onlaunch_shouldEmitPoseDataOnboard", m_shouldEmitPoseDataOnboard))
+ {
+ m_shouldEmitPoseDataOnboard = false;
+ ROS_ERROR("[CONTROLLER TABS GUI] missing parameter 'onlaunch_shouldEmitPoseDataOnboard'");
+ }
+
+
+
+
// CREATE A NODE HANDLE TO THIS GUI
ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
@@ -209,26 +231,39 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ros::NodeHandle nodeHandle_dfall_root("/dfall");
// CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
- m_poseDataSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataReceivedCallback, this);
+ m_poseDataMocapSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 3, &ControllerTabs::poseDataMocapReceivedCallback, this);
- // CREATE THE SUBSCRIBER TO THE CONTROLLER THAT IS CURRENTLY OPERATING
- // Only if this is an agent GUI
+ // CREATE THE SUBSCRIBER TO AGENT SPECIFIC TOPICS:
+ // i.e., only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
+ // THE STATE ESTIMATE FROM ONBOARD THE FLYING AGENT
+ m_poseDataOnboardSubscriber = nodeHandle_for_this_gui.subscribe("CrazyRadio/CFStateEstimate", 3, &ControllerTabs::poseDataOnboardReceivedCallback, this);
+ // THE CONTROLLER THAT IS CURRENTLY OPERATING
controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
+ // NOTE: FOR THE "poseDataMocap" AND "poseDataOnboard"
+ // SUBSCRIBERS:
+ // > The second argument is the queue_size, and the
+ // documentation states that:
+ // "If messages are arriving too fast and you are
+ // unable to keep up, roscpp will start throwing
+ // away messages."
+ // > It is clearly stated that publisher queues throw
+ // away old messages first, and subscriber queues
+ // are likely the same.
+ // > As we are only interested in latest measurement
+ // a short queue is used.
- // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
- ros::NodeHandle dfall_root_nodeHandle("/dfall");
// > Publisher for the emergency stop button
- //emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1);
+ //emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1);
// > For changes in the database that defines {agentID,cfID,flying zone} links
- //databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
- centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
+ //databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
+ centralManagerDatabaseService = nodeHandle_dfall_root.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
#endif
@@ -361,18 +396,18 @@ void ControllerTabs::setObjectNameForDisplayingPoseData( QString object_name )
#ifdef CATKIN_MAKE
-// > For the controller currently operating, received on
-// "controllerUsedSubscriber"
-void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconData)
+// > For the data from the motion capture system, received on
+// "m_poseDataMocapSubscriber"
+void ControllerTabs::poseDataMocapReceivedCallback(const dfall_pkg::ViconData& viconData)
{
m_should_search_pose_data_for_object_name_mutex.lock();
if (m_should_search_pose_data_for_object_name)
{
- for(std::vector<dfall_pkg::CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+ for(std::vector<dfall_pkg::FlyingVehicleState>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
{
- dfall_pkg::CrazyflieData pose_in_global_frame = *it;
+ dfall_pkg::FlyingVehicleState pose_in_global_frame = *it;
- if(pose_in_global_frame.crazyflieName == m_object_name_for_emitting_pose_data)
+ if(pose_in_global_frame.vehicleName == m_object_name_for_emitting_pose_data)
{
// Convert it into the local frame
@@ -387,15 +422,18 @@ void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconD
// float originZ = (area.zmin + area.zmax) / 2.0;
//pose_in_global_frame.z -= originZ;
- emit measuredPoseValueChanged(
- pose_in_global_frame.x,
- pose_in_global_frame.y,
- pose_in_global_frame.z,
- pose_in_global_frame.roll,
- pose_in_global_frame.pitch,
- pose_in_global_frame.yaw,
- pose_in_global_frame.occluded
- );
+ if (m_shouldEmitPoseDataMocap)
+ {
+ emit measuredPoseValueChanged(
+ pose_in_global_frame.x,
+ pose_in_global_frame.y,
+ pose_in_global_frame.z,
+ pose_in_global_frame.roll,
+ pose_in_global_frame.pitch,
+ pose_in_global_frame.yaw,
+ pose_in_global_frame.isValid
+ );
+ }
}
}
}
@@ -407,6 +445,29 @@ void ControllerTabs::poseDataReceivedCallback(const dfall_pkg::ViconData& viconD
+#ifdef CATKIN_MAKE
+// > For the data from the motion capture system, received on
+// "m_poseDataOnboardSubscriber"
+void ControllerTabs::poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehicleState& pose_in_local_frame)
+{
+ if (m_shouldEmitPoseDataOnboard)
+ {
+ emit measuredPoseValueChanged(
+ pose_in_local_frame.x,
+ pose_in_local_frame.y,
+ pose_in_local_frame.z,
+ pose_in_local_frame.roll,
+ pose_in_local_frame.pitch,
+ pose_in_local_frame.yaw,
+ pose_in_local_frame.isValid
+ );
+ }
+}
+#endif
+
+
+
+
// ----------------------------------------------------------------------------------
// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
@@ -646,8 +707,11 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
}
// SUBSCRIBERS
- // > For receiving message that the instant controller was changed
+
m_change_highlighted_controller_mutex.lock();
+ // > For receiving the state estimate from onboard the fying agent
+ m_poseDataOnboardSubscriber = agent_base_nodeHandle.subscribe("CrazyRadio/CFStateEstimate", 3, &ControllerTabs::poseDataOnboardReceivedCallback, this);
+ // > For receiving messages that the instant controller was changed
controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
m_change_highlighted_controller_mutex.unlock();
}
@@ -655,6 +719,7 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
{
// Unsubscribe
m_change_highlighted_controller_mutex.lock();
+ m_poseDataOnboardSubscriber.shutdown();
controllerUsedSubscriber.shutdown();
m_change_highlighted_controller_mutex.unlock();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
index eb55555bc6b40e703d2f6ec23c5d8113aea52005..a93a3d8fa82e6f48cd8f51e48b97a8e2cc2e32a9 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
@@ -625,7 +625,7 @@ void CoordinatorRow::loadCrazyflieContext()
}
// This updating of the radio only needs to be done by the actual agent's node
//ros::NodeHandle nh("CrazyRadio");
- //nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+ //nh.setParam("crazyflieAddress", m_context.crazyflieAddress);
#else
// Set the Crazyflie Name String to be a question mark
qstr_crazyflie_name.append("?");
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
index 401093fbe7b2f54dedc52a6e2c33f51e6375457e..37b301055cf06de4b73da0178a9e67f72a8f7d3e 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
@@ -43,7 +43,7 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
ui->setupUi(this);
// Hide the two red frames that are used to indcated
- // when pose data is occluded
+ // when pose data is NOT valid
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
@@ -632,10 +632,9 @@ void CsoneControllerTab::on_lineEdit_step_duration_editingFinished()
// ----------------------------------------------------------------------------------
-void CsoneControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void CsoneControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- // Check if the object is occluded for this data
- if (!occluded)
+ if (isValid)
{
// Ensure the red frames are not visible
if ( ui->red_frame_position_left->isVisible() )
@@ -645,7 +644,8 @@ void CsoneControllerTab::setMeasuredPose(float x , float y , float z , float rol
}
else
{
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->red_frame_position_left->isVisible()) )
ui->red_frame_position_left->setVisible(true);
if ( !(ui->red_frame_position_right->isVisible()) )
@@ -653,7 +653,7 @@ void CsoneControllerTab::setMeasuredPose(float x , float y , float z , float rol
}
// Pass the data through to the plotting function
- if (!occluded)
+ if (isValid)
{
newDataForPerformingStepAndPlotting(x,pitch*RAD2DEG);
}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
index 7904b231a353462038bc78e437a2ee8707ce4377..171f3803c72e5430166256b92467d9c0ddae3123 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/defaultcontrollertab.cpp
@@ -43,7 +43,7 @@ DefaultControllerTab::DefaultControllerTab(QWidget *parent) :
ui->setupUi(this);
// Hide the two red frames that are used to indcated
- // when pose data is occluded
+ // when pose data is NOT valid
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
@@ -126,9 +126,9 @@ DefaultControllerTab::~DefaultControllerTab()
// ----------------------------------------------------------------------------------
-void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- if (!occluded)
+ if (isValid)
{
// INITIALISE A STRING VARIABLE FOR ADDING THE "+"
QString qstr = "";
@@ -173,7 +173,8 @@ void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float r
}
else
{
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->red_frame_position_left->isVisible()) )
ui->red_frame_position_left->setVisible(true);
if ( !(ui->red_frame_position_right->isVisible()) )
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
index 8fc5ca827656558a05149d85dc0206b25868d239..31c95bdbcf0faae262b40e0220adca4cf7eab362 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/enablecontrollerloadyamlbar.cpp
@@ -126,6 +126,24 @@ void EnableControllerLoadYamlBar::showHideController_csone_changed()
ui->load_yaml_csone_button->setHidden( !(ui->load_yaml_csone_button->isHidden()) );
}
+bool EnableControllerLoadYamlBar::showHide_loadYamlBar_triggered()
+{
+ bool bar_isHidden;
+ if (ui->load_yaml_label->isHidden())
+ {
+ ui->load_yaml_label->show();
+ ui->widget_yamlButtons->show();
+ bar_isHidden = false;
+ }
+ else
+ {
+ ui->load_yaml_label->hide();
+ ui->widget_yamlButtons->hide();
+ bar_isHidden = true;
+ }
+ return bar_isHidden;
+}
+
// TEST MOTORS
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
index c29b832311aee31dfe59804f74483c6db8382c8d..e61244c2865eda33a47440de3c75c0be75fb3a9e 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/mainwindow.cpp
@@ -91,7 +91,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
// Hide the coordinator part of the GUI
ui->customWidget_coordinator->hide();
// And make the menu item unavailable
- ui->actionShowHide_Coordinator->setEnabled(false);
+ ui->action_showHide_Coordinator->setEnabled(false);
}
#endif
@@ -160,7 +160,7 @@ MainWindow::~MainWindow()
}
-void MainWindow::on_actionShowHide_Coordinator_triggered()
+void MainWindow::on_action_showHide_Coordinator_triggered()
{
//ui->customWidget_enableControllerLoadYamlBar->setEnabled(false);
if ( ui->customWidget_coordinator->isHidden() )
@@ -168,14 +168,32 @@ void MainWindow::on_actionShowHide_Coordinator_triggered()
ui->customWidget_coordinator->show();
ui->coordinator_to_main_panel_vertical_line->show();
QString qstr = "Hide Coordinator";
- ui->actionShowHide_Coordinator->setText(qstr);
+ ui->action_showHide_Coordinator->setText(qstr);
}
else
{
ui->customWidget_coordinator->hide();
ui->coordinator_to_main_panel_vertical_line->hide();
QString qstr = "Show Coordinator";
- ui->actionShowHide_Coordinator->setText(qstr);
+ ui->action_showHide_Coordinator->setText(qstr);
+ }
+}
+
+
+void MainWindow::on_action_showHide_loadYamlBar_triggered()
+{
+ // Trigger the function to show/hide the bar
+ bool bar_isHidden = ui->customWidget_enableControllerLoadYamlBar->showHide_loadYamlBar_triggered();
+ // Update the text of the menu item
+ if ( bar_isHidden )
+ {
+ QString qstr = "Show Yaml Buttons";
+ ui->action_showHide_loadYamlBar->setText(qstr);
+ }
+ else
+ {
+ QString qstr = "Hide Yaml Buttons";
+ ui->action_showHide_loadYamlBar->setText(qstr);
}
}
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
index 0375c8f6901d39e108bcc4bff4a0c3fad2808c48..bba7f61dc6726aec38c126a8f18f0e99d8d2f06e 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
@@ -537,9 +537,9 @@ void PickerControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithH
// ----------------------------------------------------------------------------------
-void PickerControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void PickerControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- if (!occluded)
+ if (isValid)
{
// INITIALISE A STRING VARIABLE FOR ADDING THE "+"
QString qstr = "";
@@ -583,7 +583,8 @@ void PickerControllerTab::setMeasuredPose(float x , float y , float z , float ro
}
else
{
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->frame_x_unavailable->isVisible()) )
{
ui->frame_x_unavailable ->setVisible(true);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
index 38385a277d9f12e0c75f65616fc3d0fad972e4e5..56d2dc988060445b083327e314ca175dd99f04ad 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/remotecontrollertab.cpp
@@ -43,7 +43,7 @@ RemoteControllerTab::RemoteControllerTab(QWidget *parent) :
ui->setupUi(this);
// Hide the two red frames that are used to indcated
- // when pose data is occluded
+ // when pose data is NOT valid
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
@@ -209,25 +209,17 @@ void RemoteControllerTab::on_deactivate_button_clicked()
#ifdef CATKIN_MAKE
-void RemoteControllerTab::remoteDataCallback(const dfall_pkg::CrazyflieData& objectData)
+void RemoteControllerTab::remoteDataCallback(const dfall_pkg::FlyingVehicleState& objectData)
{
- // Check if the object is occluded
- if (objectData.occluded)
+ // Check if the object date isValid
+ if (!objectData.isValid)
{
-
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->red_frame_remote->isVisible()) )
+ {
ui->red_frame_remote->setVisible(true);
-
-// // Set the column heading label to have a red background
-// // > IMPORTANT: Set the background auto fill property to true
-// ui->remote_data_label->setAutoFillBackground(true);
-// // > Get the pallette currently set for the label
-// QPalette pal = ui->remote_roll_label->palette();
-// // > Set the palette property that will change the background
-// pal.setColor(QPalette::Window, QColor(Qt::red));
-// // > Update the palette for the label
-// ui->remote_data_label->setPalette(pal);
+ }
}
else
{
@@ -237,25 +229,18 @@ void RemoteControllerTab::remoteDataCallback(const dfall_pkg::CrazyflieData& obj
ui->remote_data_yaw ->setText(QString::number( objectData.yaw * RAD2DEG, 'f', 1));
ui->remote_data_z ->setText(QString::number( objectData.z, 'f', 2));
-
// Ensure the red frames are not visible
if ( ui->red_frame_remote->isVisible() )
+ {
ui->red_frame_remote->setVisible(false);
-
-// // Set the column heading label to have a "normal" background
-// // > IMPORTANT: Set the background auto fill property to true
-// ui->remote_data_label->setAutoFillBackground(false);
-// // > Get the pallette currently set for the roll label
-// QPalette pal = ui->remote_roll_label->palette();
-// // > Update the palette for the column heading label
-// ui->remote_data_label->setPalette(pal);
+ }
}
}
#endif
#ifdef CATKIN_MAKE
-void RemoteControllerTab::remoteControlSetpointCallback(const dfall_pkg::CrazyflieData& setpointData)
+void RemoteControllerTab::remoteControlSetpointCallback(const dfall_pkg::FlyingVehicleState& setpointData)
{
ui->remote_setpoint_roll ->setText(QString::number( setpointData.roll * RAD2DEG, 'f', 1));
ui->remote_setpoint_pitch->setText(QString::number( setpointData.pitch * RAD2DEG, 'f', 1));
@@ -308,9 +293,9 @@ void RemoteControllerTab::publish_custom_button_command(int button_index , QLine
// ----------------------------------------------------------------------------------
-void RemoteControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void RemoteControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- if (!occluded)
+ if (isValid)
{
// INITIALISE A STRING VARIABLE FOR ADDING THE "+"
QString qstr = "";
@@ -337,7 +322,8 @@ void RemoteControllerTab::setMeasuredPose(float x , float y , float z , float ro
}
else
{
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->red_frame_position_left->isVisible()) )
ui->red_frame_position_left->setVisible(true);
if ( !(ui->red_frame_position_right->isVisible()) )
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
index 161b03ac9dad95a0aff307511e02adc9d18d9a3a..53d3b54ddd13f0a6288b8cd22bfaaeacc1e68a4a 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/rosNodeThread_for_flyingAgentGUI.cpp
@@ -82,7 +82,7 @@ bool rosNodeThread::init()
ros::Time::init();
ros::NodeHandle nh("~");
- //m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
+ //m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 3, &rosNodeThread::messageCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
index 64207a97bc6be97ef37307dffa36f2ffa8044cad..6896bd7967aed13a4b08e711afe8d55adc9d5988 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/studentcontrollertab.cpp
@@ -43,7 +43,7 @@ StudentControllerTab::StudentControllerTab(QWidget *parent) :
ui->setupUi(this);
// Hide the two red frames that are used to indcated
- // when pose data is occluded
+ // when pose data is NOT valid
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
@@ -205,9 +205,9 @@ void StudentControllerTab::on_custom_button_5_clicked()
// ----------------------------------------------------------------------------------
-void StudentControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void StudentControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- if (!occluded)
+ if (isValid)
{
// INITIALISE A STRING VARIABLE FOR ADDING THE "+"
QString qstr = "";
@@ -252,7 +252,8 @@ void StudentControllerTab::setMeasuredPose(float x , float y , float z , float r
}
else
{
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->red_frame_position_left->isVisible()) )
ui->red_frame_position_left->setVisible(true);
if ( !(ui->red_frame_position_right->isVisible()) )
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
index 24f9fb07fedae441a7de3f94e035e811fc603062..0d963a498f523047b83804710654c1cbbc4636e6 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/templatecontrollertab.cpp
@@ -43,7 +43,7 @@ TemplateControllerTab::TemplateControllerTab(QWidget *parent) :
ui->setupUi(this);
// Hide the two red frames that are used to indcated
- // when pose data is occluded
+ // when pose data is NOT valide
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
@@ -191,9 +191,9 @@ void TemplateControllerTab::on_custom_button_3_clicked()
// ----------------------------------------------------------------------------------
-void TemplateControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void TemplateControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- if (!occluded)
+ if (isValid)
{
// INITIALISE A STRING VARIABLE FOR ADDING THE "+"
QString qstr = "";
@@ -220,7 +220,8 @@ void TemplateControllerTab::setMeasuredPose(float x , float y , float z , float
}
else
{
- // Make visible the red frames to indicate occluded
+ // Make visible the red frames to indicate that
+ // measurement is NOT valid
if ( !(ui->red_frame_position_left->isVisible()) )
ui->red_frame_position_left->setVisible(true);
if ( !(ui->red_frame_position_right->isVisible()) )
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
index baa47e7b143d517fe4eb0bf530d660786450994d..b2d401b726ffb595e4f2beeafd983efae5b1ab32 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
@@ -231,7 +231,7 @@ void TopBanner::loadCrazyflieContext(int ID_to_request_from_database , int emit_
}
// This updating of the radio only needs to be done by the actual agent's node
//ros::NodeHandle nh("CrazyRadio");
- //nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+ //nh.setParam("crazyflieAddress", m_context.crazyflieAddress);
#else
// Set the Crazyflie Name String to be a question mark
qstr_crazyflie_name.append("?");
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
index f6578b3217df7ee6b9f04c7250610a5fb9e88b16..04f58ca425e320d586a6c83534c1a8fc483401f9 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tuningcontrollertab.cpp
@@ -139,9 +139,9 @@ TuningControllerTab::~TuningControllerTab()
// ----------------------------------------------------------------------------------
-void TuningControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+void TuningControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool isValid)
{
- if (!occluded)
+ if (isValid)
{
m_gain_setpoint_mutex.lock();
@@ -203,7 +203,8 @@ void TuningControllerTab::setMeasuredPose(float x , float y , float z , float ro
}
else
{
-// // Make visible the red frames to indicate occluded
+// // Make visible the red frames to indicate that
+// // measurement is NOT valid
// if ( !(ui->red_frame_position_left->isVisible()) )
// ui->red_frame_position_left->setVisible(true);
// if ( !(ui->red_frame_position_right->isVisible()) )
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 d26b198cdb27abf3cf29ff3a9da7c182b1f68840..008ce939198a1e90be0e8c3dff5c9b70def2f5de 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
@@ -41,7 +41,7 @@
#ifdef CATKIN_MAKE
// Include the Crazyflie Data Struct
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
#else
@@ -63,7 +63,7 @@ using namespace dfall_pkg;
class crazyFly : public QGraphicsSvgItem
{
public:
- explicit crazyFly(const CrazyflieData* p_crazyfly_msg, QString filename, QGraphicsItem * parent = 0);
+ explicit crazyFly(const FlyingVehicleState* p_crazyfly_msg, QString filename, QGraphicsItem * parent = 0);
~crazyFly();
QRectF boundingRect() const;
@@ -71,7 +71,7 @@ public:
const QStyleOptionGraphicsItem * option,
QWidget * widget);
- void updateCF(const CrazyflieData* p_crazyfly_msg);
+ void updateCF(const FlyingVehicleState* p_crazyfly_msg);
std::string getName();
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
index 5d17177cbf0fff1ceafa4143166a2ed65946b9c7..9b4cfae589e46b806c973306b73714ad3d85f4fe 100755
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/mainguiwindow.h
@@ -206,7 +206,7 @@ private:
#ifdef CATKIN_MAKE
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
- void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+ void getParameterFloatVectorKnownLength(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);
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h
index 1a4f037cd6da979a8966426a1605ef1a639889b0..e74a75eb414e7e1a5e83c39e9c5bbfac19d04bdf 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/systemConfigGUI/include/rosNodeThread_for_systemConfigGUI.h
@@ -44,7 +44,7 @@
#include <ros/ros.h>
#include <ros/network.h>
#include "dfall_pkg/UnlabeledMarker.h"
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/ViconData.h"
using namespace dfall_pkg;
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 5bea8cf634de9e08be35f7f832bcd47ca0c9f8f9..417019b3eed75c7a2d3a68353e5634caa83468d4 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
@@ -36,7 +36,7 @@
#include <QBrush>
-crazyFly::crazyFly(const CrazyflieData* p_crazyfly_msg, QString filename, QGraphicsItem * parent)
+crazyFly::crazyFly(const FlyingVehicleState* p_crazyfly_msg, QString filename, QGraphicsItem * parent)
: QGraphicsSvgItem(filename)
{
updateCF(p_crazyfly_msg);
@@ -83,11 +83,11 @@ std::string crazyFly::getName()
}
-void crazyFly::updateCF(const CrazyflieData* p_crazyfly_msg)
+void crazyFly::updateCF(const FlyingVehicleState* p_crazyfly_msg)
{
- m_occluded = p_crazyfly_msg->occluded;
+ m_occluded = !(p_crazyfly_msg->isValid);
- m_name = p_crazyfly_msg->crazyflieName;
+ m_name = p_crazyfly_msg->vehicleName;
if(!m_occluded) //if it is occluded, the info we got is useless
{
m_x = p_crazyfly_msg->x;
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 a93637d1cd0e41b52681f7678764f7596ae011ff..6e249d795bd68bc049307c6ffb1d4d8aada66698 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
@@ -417,7 +417,7 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
int index_name_found;
for(int j = 0; j < crazyfly_vector_size_before; j++)
{
- if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
+ if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].vehicleName)
{
name_found = true; // name found. This can only happen once per i-iteration, names are unique
index_name_found = j; // index in already existing vector, to update it later (really needed?)
@@ -431,7 +431,7 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
else //name not found, newly arrived, add it to the vector
{
// now, if name follows our format, put the corresponding number. If not, put the unknown image
- std::string s = p_msg->crazyflies[i].crazyflieName;
+ std::string s = p_msg->crazyflies[i].vehicleName;
std::smatch m;
std::regex e ("CF([0-9]{2})");
@@ -473,7 +473,6 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
{
if(crazyflies_vector[i]->isOccluded())
{
- // ROS_INFO("===================OCCLUDED");
if(crazyflies_vector[i]->isAddedToScene())
{
scene->removeItem(crazyflies_vector[i]);
@@ -500,7 +499,7 @@ void MainGUIWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to
bool name_found = false;
for(int i = 0; i < p_msg->crazyflies.size(); i++)
{
- if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].crazyflieName)
+ if(crazyflies_vector[j]->getName() == p_msg->crazyflies[i].vehicleName)
{
name_found = true;
}
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index ae7a2c09405e26ad52d053f69eccb3406264b13b..fb0e3cf53fcea56b250d0eaceb46e3af1fbbee4c 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -33,21 +33,32 @@
# ----------------------------------------------------------------------------------
-import roslib; roslib.load_manifest('dfall_pkg')
+# Import the ROS library package
+import roslib
+# Load the "dfall package" manifest for ROS
+roslib.load_manifest('dfall_pkg')
+# Import the ROS-Python package
import rospy
-from std_msgs.msg import Int32
+# Import standard messages
+from std_msgs.msg import Int32
+# Import dfall_pkg messages
from dfall_pkg.msg import ControlCommand
from dfall_pkg.msg import IntWithHeader
+from dfall_pkg.msg import FlyingVehicleState
+from dfall_pkg.msg import GyroMeasurements
+# Import dfall_pkg services
from dfall_pkg.srv import IntIntService
-
# General import
import time, sys
import struct
import logging
-import rosbag
-from rospkg import RosPack
+# Import the ROS bag package for logging
+#import rosbag
+#from rospkg import RosPack
+
+# Import the ROS standard message types
from std_msgs.msg import Float32
from std_msgs.msg import String
@@ -62,7 +73,7 @@ from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
import cflib.drivers.crazyradio
-# Logging import(*
+# Import the package for logging data back from the Crazyflie
from cflib.crazyflie.log import LogConfig
# Logging settings
@@ -72,9 +83,25 @@ logging.basicConfig(level=logging.ERROR)
# CONTROLLER_ANGLE = 1
# CONTROLLER_RATE = 0
-CF_COMMAND_TYPE_MOTORS = 8
-CF_COMMAND_TYPE_RATE = 9
-CF_COMMAND_TYPE_ANGLE = 10
+# CF_COMMAND_TYPE_MOTORS = 8
+# CF_COMMAND_TYPE_RATE = 9
+# CF_COMMAND_TYPE_ANGLE = 10
+
+CF_ONBOARD_CONTROLLER_MODE_OFF = 0
+CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE = 1
+CF_ONBOARD_CONTROLLER_MODE_ANGLE = 2
+CF_ONBOARD_CONTROLLER_MODE_VELOCITY = 3
+CF_ONBOARD_CONTROLLER_MODE_POSITION = 4
+
+# CF_PACKET_DECODER_STOP_TYPE = 0
+# CF_PACKET_DECODER_VELOCITY_WORLD_TYPE = 1
+# CF_PACKET_DECODER_Z_DISTANCE_TYEP = 2
+# CF_PACKET_DECODER_CPPM_EMU_TYPE = 3
+# CF_PACKET_DECODER_ALT_HOLD_TYPE = 4
+# CF_PACKET_DECODER_HOVER_TYPE = 5
+# CF_PACKET_DECODER_FULL_STATE_TYPE = 6
+# CF_PACKET_DECODER_POSITION_TYPE = 7
+CF_PACKET_DECODER_DFALL_TYPE = 8
RAD_TO_DEG = 57.296
@@ -99,11 +126,27 @@ CMD_CRAZYFLY_TAKE_OFF = 11
CMD_CRAZYFLY_LAND = 12
CMD_CRAZYFLY_MOTORS_OFF = 13
-rp = RosPack()
-record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
-rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
-rospy.loginfo(record_file)
-bag = rosbag.Bag(record_file, 'w')
+# Types of flying vehicle states:
+#FLYING_VEHICLE_STATE_TYPE_NONE = 0
+#FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT = 1
+FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE = 2
+#FLYING_VEHICLE_STATE_TYPE_12_STATE_ESTIMATE = 3
+
+# Flying states
+STATE_MOTORS_OFF = 1
+#STATE_TAKE_OFF = 2
+#STATE_FLYING = 3
+#STATE_LAND = 4
+#STATE_UNAVAILABLE = 5
+
+
+
+# Open the ROS bag for logging
+#rp = RosPack()
+#record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
+#rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
+#rospy.loginfo(record_file)
+#bag = rosbag.Bag(record_file, 'w')
class CrazyRadioClient:
"""
@@ -113,19 +156,27 @@ class CrazyRadioClient:
"""
def __init__(self):
- # Setpoints to be sent to the CrazyFlie
- self.roll = 0.0
- self.pitch = 0.0
- self.yaw = 0.0
- self.motor1cmd = 0.0
- self.motor2cmd = 0.0
- self.motor3cmd = 0.0
- self.motor4cmd = 0.0
+ # INITIALISE THE PROPERTIES OF THE OBJECT
+ # > For the status of the radio link
self._status = DISCONNECTED
+ # > For the addess of the radio link
self.link_uri = ""
-
- self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
- self.FlyingAgentClient_command_pub = rospy.Publisher('FlyingAgentClient/Command', IntWithHeader, queue_size=1)
+ # > For the name of the connected crazyflie
+ self.crazyflie_name = ""
+ # > For keeping track of the "flying state"
+ # of the "FlyingAgentClient"
+ self._flyingState_of_flyingAgentClient = STATE_MOTORS_OFF
+
+ # > INIIALISE PUBLISHERS
+ # > For informing the network of the status of
+ # the radio link
+ self.radio_status_publisher = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1)
+ # > For publishing commands on behalf of the
+ # FlyingAgentClient, only used for publishing
+ # "motor off" commands
+ self.flyingAgentClient_command_publisher = rospy.Publisher('FlyingAgentClient/Command', IntWithHeader, queue_size=1)
+
+ # PAUSE SHORTLY FOR THE PUBLISHERS TO BE REGISTERED IN ROS
time.sleep(1.0)
# Initialize the CrazyFlie and add callbacks
@@ -148,13 +199,16 @@ class CrazyRadioClient:
def change_status_to(self, new_status):
print "[CRAZY RADIO] status changed to: %s" % new_status
self._status = new_status
- self.status_pub.publish(new_status)
+ self.radio_status_publisher.publish(new_status)
def get_status(self):
return self._status
def update_link_uri(self):
- self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
+ self.link_uri = "radio://" + rospy.get_param("~crazyflieAddress")
+
+ def update_crazyflie_name(self):
+ self.crazyflie_name = rospy.get_param("~crazyflieName")
def connect(self):
# update link from ros params
@@ -167,60 +221,181 @@ class CrazyRadioClient:
def disconnect(self):
print "[CRAZY RADIO] sending Motors OFF command before disconnecting"
- self._send_to_commander_motor(0, 0, 0, 0)
+ cf_client._send_to_commander(0, 0, 0, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0)
# change state to motors OFF
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
- self.FlyingAgentClient_command_pub.publish(msg)
+ self.flyingAgentClient_command_publisher.publish(msg)
time.sleep(0.1)
print "[CRAZY RADIO] Disconnecting from %s" % self.link_uri
self._cf.close_link()
self.change_status_to(DISCONNECTED)
- def _data_received_callback(self, timestamp, data, logconf):
- #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
+
+
+ def _data_received_battery_callback(self, timestamp, data, logconf):
+ # Initialise the variable for the data
batteryVolt = Float32()
- stabilizerYaw = Float32()
- stabilizerPitch = Float32()
- stabilizerRoll = Float32()
+ # Retrieve the values from the data packet received
batteryVolt.data = data["pm.vbat"]
- stabilizerYaw.data = data["stabilizer.yaw"]
- stabilizerPitch.data = data["stabilizer.pitch"]
- bag.write('batteryVoltage', batteryVolt)
- bag.write('stabilizerYaw', stabilizerYaw)
- bag.write('stabilizerPitch', stabilizerPitch)
- bag.write('stabilizerRoll', stabilizerRoll)
-
- #publish battery voltage for GUI
- #cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
- # print "batteryVolt: %s" % batteryVolt
+
+ # Write data to the ROS bag
+ #bag.write('batteryVoltage', batteryVolt)
+
+ # Print out one value for DEBUGGING
+ #print "[CRAZY RADIO] DEBUGGING, received pm.vbat = %f" % batteryVolt.data
+
+ # Publish the battery voltage
cfbattery_pub.publish(batteryVolt)
+ def _data_received_stateEstimate_callback(self, timestamp, data, logconf):
+ # Perform safety check if required
+ if (isEnabled_strictSafety):
+ # Estimate the height at the next measurement
+ height_at_next_measurement = data["stateEstimateZ.z"] / 1000.0 + (cfStateEstimate_polling_period / 1000.0) * (data["stateEstimateZ.vz"] / 1000.0)
+ # Turn-off if too high
+ if (height_at_next_measurement > maxHeight_for_strictSafety_meters):
+ # Publish a motors OFF command
+ msg = IntWithHeader()
+ msg.shouldCheckForAgentID = False
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF
+ self.flyingAgentClient_command_publisher.publish(msg)
+ # Inform the user
+ rospy.logerr("[CRAZY RADIO] Height safety check failed, measured = %f, max allowed = %f" % (height_at_next_measurement, maxHeight_for_strictSafety_meters))
+
+ # Initialise the variable for the flying vehicle state
+ cfStateEstimate = FlyingVehicleState()
+
+ # Retrieve the values from the data packet received
+ # > (x,y,z) positions
+ cfStateEstimate.x = data["stateEstimateZ.x"] / 1000.0
+ cfStateEstimate.y = data["stateEstimateZ.y"] / 1000.0
+ cfStateEstimate.z = data["stateEstimateZ.z"] / 1000.0
+ # > (x,y,z) velocities
+ cfStateEstimate.vx = data["stateEstimateZ.vx"] / 1000.0
+ cfStateEstimate.vy = data["stateEstimateZ.vy"] / 1000.0
+ cfStateEstimate.vz = data["stateEstimateZ.vz"] / 1000.0
+ # > (x,y,z) accelerations
+ #cfStateEstimate.ax = data["stateEstimateZ.ax"] / 1000.0
+ #cfStateEstimate.ay = data["stateEstimateZ.ay"] / 1000.0
+ #cfStateEstimate.az = data["stateEstimateZ.az"] / 1000.0
+ # > (roll,pitch,yaw) angles
+ cfStateEstimate.roll = data["stateEstimateZ.roll"] / 1000.0
+ cfStateEstimate.pitch = -data["stateEstimateZ.pitch"] / 1000.0
+ cfStateEstimate.yaw = data["stateEstimateZ.yaw"] / 1000.0
+ # > (roll,pitch,yaw) anglular rates (direct from gryo)
+ cfStateEstimate.rollRate = data["stateEstimateZ.rateRoll"] / 1000.0
+ cfStateEstimate.pitchRate = -data["stateEstimateZ.ratePitch"] / 1000.0
+ cfStateEstimate.yawRate = data["stateEstimateZ.rateYaw"] / 1000.0
+
+ # Print out one value for DEBUGGING
+ #print "[CRAZY RADIO] received (z,vz) = ( %6.3f , %6.3f )" %( cfStateEstimate.z , cfStateEstimate.vz )
+ #print "[CRAZY RADIO] received (r,p,y) = ( %6.3f , %6.3f , %6.3f )" %( cfStateEstimate.roll*RAD_TO_DEG , cfStateEstimate.pitch*RAD_TO_DEG , cfStateEstimate.yaw*RAD_TO_DEG )
+
+ # Fill in the remaining details:
+ # > For the type
+ cfStateEstimate.type = FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE
+ # > For the name
+ cfStateEstimate.vehicleName = self.crazyflie_name
+ # > For the validity flag
+ cfStateEstimate.isValid = True
+ # > For the acquiring time
+ cfStateEstimate.acquiringTime = 0.0
+
+ # Publish the state estimate
+ cfstateEstimate_pub.publish(cfStateEstimate)
+
+
+
+ # Initialise the variable for the gyroscope data
+ gyroMeasurements = GyroMeasurements()
+
+ # Retrieve the values from the data packet received
+ # > (roll,pitch,yaw) anglular rates (direct from gryo)
+ gyroMeasurements.rollrate = data["stateEstimateZ.rateRoll"] / 1000.0
+ gyroMeasurements.pitchrate = -data["stateEstimateZ.ratePitch"] / 1000.0
+ gyroMeasurements.yawrate = data["stateEstimateZ.rateYaw"] / 1000.0
+
+ # Fill in the name
+ gyroMeasurements.vehicleName = self.crazyflie_name
+
+ # Print out one value for DEBUGGING
+ #print "[CRAZY RADIO] gyro (r,p,y) = ( %6.3f , %6.3f , %6.3f )" %( gyroMeasurements.rollrate*RAD_TO_DEG , gyroMeasurements.pitchrate*RAD_TO_DEG , gyroMeasurements.yawrate*RAD_TO_DEG )
+
+ # Publish the gyroscope measurements
+ cfgyroMeasurements_pub.publish(gyroMeasurements)
+
+
+
def _logging_error(self, logconf, msg):
print "[CRAZY RADIO] Error when logging %s" % logconf.name
- # def _init_logging(self):
+
def _start_logging(self):
- self.logconf = LogConfig("LoggingTest", battery_polling_period) # second variable is period in ms
- self.logconf.add_variable("stabilizer.roll", "float");
- self.logconf.add_variable("stabilizer.pitch", "float");
- self.logconf.add_variable("stabilizer.yaw", "float");
- self.logconf.add_variable("pm.vbat", "float");
-
- self._cf.log.add_config(self.logconf)
- if self.logconf.valid:
- self.logconf.data_received_cb.add_callback(self._data_received_callback)
- self.logconf.error_cb.add_callback(self._logging_error)
- print "[CRAZY RADIO] logconf valid"
+
+ # A "LOGGING GROUP" FOR THE BATTERY VOLTAGE:
+
+ # Initialise a log config object
+ self.logconf_battery = LogConfig("LoggingBattery", period_in_ms=battery_polling_period)
+ # Add the varaibles to be logged
+ self.logconf_battery.add_variable("pm.vbat", "float");
+ # Add the log config to the crazyflie object
+ self._cf.log.add_config(self.logconf_battery)
+ if self.logconf_battery.valid:
+ self.logconf_battery.data_received_cb.add_callback(self._data_received_battery_callback)
+ self.logconf_battery.error_cb.add_callback(self._logging_error)
+ print "[CRAZY RADIO] The log config for the battery voltage is valid"
+ else:
+ print "[CRAZY RADIO] The log config for the battery voltage is invalid"
+
+ # A "LOGGING GROUP" FOR THE STATE ESTIMATE:
+
+ # Initialise a log config object
+ self.logconf_stateEstimate = LogConfig("LoggingStateEstimate", period_in_ms=cfStateEstimate_polling_period)
+ # Add the varaibles to be logged
+ # > (x,y,z) positions
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.x", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.y", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.z", "int16_t");
+ # > (x,y,z) velocities
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.vx", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.vy", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.vz", "int16_t");
+ # > (x,y,z) accelerations
+ #self.logconf_stateEstimate.add_variable("stateEstimateZ.ax", "int16_t");
+ #self.logconf_stateEstimate.add_variable("stateEstimateZ.ay", "int16_t");
+ #self.logconf_stateEstimate.add_variable("stateEstimateZ.az", "int16_t");
+ # > (roll,pitch,yaw) angles
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.roll", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.pitch", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.yaw", "int16_t");
+ # > (roll,pitch,yaw) anglular rates (direct from gryo)
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.rateRoll", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.ratePitch", "int16_t");
+ self.logconf_stateEstimate.add_variable("stateEstimateZ.rateYaw", "int16_t");
+ # Add the log config to the crazyflie object
+ self._cf.log.add_config(self.logconf_stateEstimate)
+ if self.logconf_stateEstimate.valid:
+ self.logconf_stateEstimate.data_received_cb.add_callback(self._data_received_stateEstimate_callback)
+ self.logconf_stateEstimate.error_cb.add_callback(self._logging_error)
+ print "[CRAZY RADIO] The log config for the state estimate is valid"
else:
- print "[CRAZY RADIO] logconf invalid"
+ print "[CRAZY RADIO] The log config for the state estimate is invalid"
+
+
+ # START THE LOGGING FOR ALL THE "LOGGING GROUPS" ADDED
+ # > For the battery voltage
+ self.logconf_battery.start()
+ print "[CRAZY RADIO] Started the log config for the battery voltage"
+ # > For the state estimate
+ self.logconf_stateEstimate.start()
+ print "[CRAZY RADIO] Started the log config for the state estimate"
+
- self.logconf.start()
- print "[CRAZY RADIO] logconf start"
def _connected(self, link_uri):
"""
@@ -232,7 +407,7 @@ class CrazyRadioClient:
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
- cf_client.FlyingAgentClient_command_pub.publish(msg)
+ cf_client.flyingAgentClient_command_publisher.publish(msg)
rospy.loginfo("[CRAZY RADIO] Connection to %s successful: " % link_uri)
# Config for Logging
@@ -240,7 +415,6 @@ class CrazyRadioClient:
-
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
@@ -258,40 +432,100 @@ class CrazyRadioClient:
self.change_status_to(DISCONNECTED)
rospy.logwarn("[CRAZY RADIO] Disconnected from %s" % link_uri)
- # change state to motors OFF
+ # CHANGE THE STATE TO "MOTORS OFF"
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
- self.FlyingAgentClient_command_pub.publish(msg)
-
- self.logconf.stop()
- rospy.loginfo("logconf stopped")
- self.logconf.delete()
- rospy.loginfo("logconf deleted")
-
- def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
+ self.flyingAgentClient_command_publisher.publish(msg)
+
+ # STOP AND DELETE ALL THE "LOGGING GROUPS"
+ # > For the battery voltage
+ self.logconf_battery.stop()
+ rospy.loginfo("The log config for the battery voltage was stopped")
+ self.logconf_battery.delete()
+ rospy.loginfo("The log config for the battery voltage was deleted")
+ # > For the state estimate
+ self.logconf_stateEstimate.stop()
+ rospy.loginfo("The log config for the state estimate was stopped")
+ self.logconf_stateEstimate.delete()
+ rospy.loginfo("The log config for the state estimate was deleted")
+
+
+
+ def _send_to_commander(self, cmd1, cmd2, cmd3, cmd4, x_mode , x_value, y_mode, y_value, z_mode, z_value, yaw_mode, yaw_value ):
+ # CONSTRUCT THE CONDENSED 16-BIT INTEGER THAT ENCODES
+ # THE REQUESTED {x,y,z,yaw} MODES
+ # > Each mode is encoded using 3 bits, stacked together as follows:
+ # ---------------------------------------------------------------
+ # |BIT | 15 14 13 12 | 11 10 9 | 8 7 6 | 5 4 3 | 2 1 0 |
+ # |DESC. | free bits | yaw_mode | z_mode | y_mode | x_mode |
+ # ---------------------------------------------------------------
+ # > The python bitwise operators "<<" and "|" are used
+ # to construct the 16-bit "cumulative" mode
+ modes_as_uint16 = (yaw_mode << 9) | (z_mode << 6) | (y_mode << 3) | (x_mode << 0)
+ #
+ # > To test the behaviour of this command, enter the following
+ # into a python shell:
+ # >>> modes = (4 << 9) | (2 << 6) | (1 << 3) | (7 << 0)
+ # >>> "{0:b}".format(modes)
+ # > The second line should display the result '100010001111'
+ # which encodes:
+ # --------------------------------------------------
+ # |BIT | 11 10 9 | 8 7 6 | 5 4 3 | 2 1 0 |
+ # |O or 1 | 1 0 0 | 0 1 0 | 0 0 1 | 1 1 1 |
+ # |Value | mode=4 | mode=2 | mode=1 | mode=7 |
+ # --------------------------------------------------
+ #
+ # > To extract a mode, an example code snippet is:
+ # >>> yaw_mode = (modes >> 9) & 7
+ # > The bitwise & operator with the number 7 serves to
+ # select only the right-hand most 3 bits of (modes >> 9),
+ # the last 3 bits of the variable modes after is has been
+ # shifted to the right by 9 bits.
+ # > This syntax works in both python and plain-c
+
+
+
+ # CONSTRUCT AND SEND THE COMMANDER PACKET
+ # > See hints at end of this file for an explanation
+ # of the struct packing format, i.e., of '<HHHHHffff'
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHH', CF_COMMAND_TYPE_MOTORS, cmd1, cmd2, cmd3, cmd4)
+ pk.data = struct.pack('<BHHHHHffff', CF_PACKET_DECODER_DFALL_TYPE, modes_as_uint16, cmd1, cmd2, cmd3, cmd4, x_value, y_value, z_value, yaw_value)
self._cf.send_packet(pk)
- def _send_to_commander_rate(self, cmd1, cmd2, cmd3, cmd4, roll_rate, pitch_rate, yaw_rate):
- pk = CRTPPacket()
- pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_RATE, cmd1, cmd2, cmd3, cmd4, roll_rate * RAD_TO_DEG, pitch_rate * RAD_TO_DEG, yaw_rate * RAD_TO_DEG)
- self._cf.send_packet(pk)
+ # NOTES ON THE PACKING OF THE "pk.data" STRUCT
+ # > The "B" is the decoder type packed into 1 btye
+ # > The first "H" is the condense 2-byte integer
+ # of four onboard controller modes
+ # > The next four "H" are the 2-byte per motor commands
+ # > The four "f" are the setpoints for each onboard controller
+ # > Hence the total size of the data is:
+ # sizs('HHHHHffff') = 5*2bytes + 4*4bytes = 26bytes
+ # > This leaves 3 spare bytes before reaching the maximum
+ # of 29 bytes as specified in the comment below
+
+ # DESCRIPTION OF THE CRAZYFLIE PACKETS
+ # > This is taken directly from the file
+ # "crtp_commander_generic.c" in the
+ # Crazyflie firmware
+ #
+ # The generic commander format contains a packet type and data that has to be
+ # decoded into a setpoint_t structure. The aim is to make it future-proof
+ # by easily allowing the addition of new packets for future use cases.
+ #
+ # The packet format is:
+ # +------+==========================+
+ # | TYPE | DATA |
+ # +------+==========================+
+ #
+ # The type is defined bellow together with a decoder function that should take
+ # the data buffer in and fill up a setpoint_t structure.
+ # The maximum data size is 29 bytes.
+
+
- def _send_to_commander_angle(self, cmd1, cmd2, cmd3, cmd4, roll, pitch, yaw):
- pk = CRTPPacket()
- pk.port = CRTPPort.COMMANDER_GENERIC
- pk.data = struct.pack('<BHHHHfff', CF_COMMAND_TYPE_ANGLE, cmd1, cmd2, cmd3, cmd4, roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG)
- self._cf.send_packet(pk)
- # def _send_to_commander(self,roll, pitch, yaw, thrust, cmd1, cmd2, cmd3, cmd4, mode):
- # pk = CRTPPacket()
- # pk.port = CRTPPort.COMMANDER
- # pk.data = struct.pack('<fffHHHHHH', roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode)
- # self._cf.send_packet(pk)
def crazyRadioCommandCallback(self, msg):
"""Callback to tell CrazyRadio to reconnect"""
@@ -317,10 +551,10 @@ class CrazyRadioClient:
self.connect()
elif self.get_status() == CONNECTING:
print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTING)"
- #self.status_pub.publish(CONNECTING)
+ #self.radio_status_publisher.publish(CONNECTING)
elif self.get_status() == CONNECTED:
print "[CRAZY RADIO] received command to CONNECT (current status is CONNECTED)"
- #self.status_pub.publish(CONNECTED)
+ #self.radio_status_publisher.publish(CONNECTED)
elif msg.data == CMD_DISCONNECT:
if self.get_status() == CONNECTED:
@@ -328,10 +562,10 @@ class CrazyRadioClient:
self.disconnect()
elif self.get_status() == CONNECTING:
print "[CRAZY RADIO] received command to DISCONNECT (current status is CONNECTING)"
- #self.status_pub.publish(CONNECTING)
+ #self.radio_status_publisher.publish(CONNECTING)
elif self.get_status() == DISCONNECTED:
print "[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
- #self.status_pub.publish(DISCONNECTED)
+ #self.radio_status_publisher.publish(DISCONNECTED)
@@ -342,23 +576,148 @@ class CrazyRadioClient:
+ def flyingStateCallback(self, msg):
+ """Callback for keeping track of the flying state"""
-def controlCommandCallback(data):
- """Callback for controller actions"""
- #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
+ # > This is used to know when to reset the Crazyflie's
+ # onboard Kalman filter.
+ # > The filter is reset when the state changes from
+ # "motors off" to something else
+ # > This is required because the onboard state estimate
+ # tends to drift when the Crazyflie stays on the ground
+ # for an even a short amount of time
- #cmd1..4 must not be 0, as crazyflie onboard controller resets!
- #pitch and yaw are inverted on crazyflie controller
- if data.onboardControllerType == CF_COMMAND_TYPE_MOTORS:
- cf_client._send_to_commander_motor(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4)
+ # The data in this message is always relevant because
+ # of the namespace of the message, hence there is no
+ # need to check the header.
- elif data.onboardControllerType == CF_COMMAND_TYPE_RATE:
- cf_client._send_to_commander_rate(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
+ # Get the new state from the message
+ new_flying_state = msg.data
+
+ # Get the current flying state into a local variable
+ current_flying_state = self._flyingState_of_flyingAgentClient
+
+ # Only consider the command if it is relevant
+ if ( \
+ (current_flying_state==STATE_MOTORS_OFF)
+ and \
+ not(new_flying_state==STATE_MOTORS_OFF) \
+ ):
+ # Call the CrazyRadio function that sets the
+ # paramter for reseting the onboard estimate
+ self._cf.param.set_value("kalman.resetEstimation", "1")
+ # Inform the user
+ #print "[CRAZY RADIO] reqested the Crazyflie to reset the onboard estimate"
- elif data.onboardControllerType == CF_COMMAND_TYPE_ANGLE:
- cf_client._send_to_commander_angle(data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.roll, -data.pitch, -data.yaw)
- # cf_client._send_to_commander(data.roll, -data.pitch, -data.yaw, 0, data.motorCmd1, data.motorCmd2, data.motorCmd3, data.motorCmd4, data.onboardControllerType)
+ # Update the class variable for keeping track of the
+ # current state
+ self._flyingState_of_flyingAgentClient = new_flying_state
+
+
+# END OF: class CrazyRadioClient:
+# ============================= #
+
+
+# ============================= #
+# CALL BACK WHEN A MESSAGE IS RECEIVED ON:
+# "FlyingAgentClient/ControlCommand"
+def controlCommandCallback(data):
+ """Callback for controller actions"""
+ # NOTE: cmd{1,...,4} must NOT be all 0, as this causes the
+ # crazyflie onboard controller to reset!
+ # TODO: check if this note is still true!
+
+ # CHECK THE X-CONTROLLER MODE
+ # > Because the pitch is inverted on the crazyflie
+ # > And because the angles need to be converted to degrees
+ if ( \
+ (data.xControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+ or \
+ (data.xControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+ ):
+ # Negate the setpoint and convert to degrees
+ x_controller_setpoint = -data.xControllerSetpoint * RAD_TO_DEG;
+ #
+ else:
+ # Take the setpoint as is
+ x_controller_setpoint = data.xControllerSetpoint;
+
+ # CHECK THE Y-CONTROLLER MODE
+ # > Because the pitch is inverted on the crazyflie
+ # > And because the angles need to be converted to degrees
+ if ( \
+ (data.yControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+ or \
+ (data.yControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+ ):
+ # Convert the setpoint to degrees
+ y_controller_setpoint = data.yControllerSetpoint * RAD_TO_DEG;
+ #
+ else:
+ # Take the setpoint as is
+ y_controller_setpoint = data.yControllerSetpoint;
+
+ # CHECK THE Z-CONTROLLER MODE
+ # > Because the angle and angular rate modes are not allowed
+ if ( \
+ (data.zControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+ or \
+ (data.zControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+ ):
+ # Turn the z-controller off and inform the user
+ rospy.logerr("[CRAZY RADIO] z controller requested disallowed mode, now turning OFF the z-controller.")
+ z_controller_mode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ z_controller_setpoint = 0.0;
+ #
+ else:
+ # Take the mode and setpoint as is
+ z_controller_mode = data.zControllerMode;
+ z_controller_setpoint = data.zControllerSetpoint;
+
+ # CHECK THE YAW-CONTROLLER MODE
+ # > Because the yaw is inverted on the crazyflie
+ # > And because the angles need to be converted to degrees
+ # > And because the position and velocity modes are not allowed
+ if ( \
+ (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE) \
+ or \
+ (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_ANGLE) \
+ ):
+ # Negate the setpoint and convert to degrees
+ yaw_controller_mode = data.yawControllerMode;
+ yaw_controller_setpoint = data.yawControllerSetpoint * RAD_TO_DEG;
+ #
+ elif (data.yawControllerMode==CF_ONBOARD_CONTROLLER_MODE_OFF):
+ # Set the setpoint to zero
+ yaw_controller_mode = data.yawControllerMode;
+ yaw_controller_setpoint = 0.0;
+ #
+ else:
+ # Set the yaw-controller to zero angle setpoint and inform the user
+ rospy.logerr("[CRAZY RADIO] yaw controller requested disallowed mode, now turning to ANGULAR_RATE mode with zero setpoint.")
+ yaw_controller_mode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ yaw_controller_setpoint = 0.0;
+
+
+ # PASS THE COMMAND TO THE SENDING FUNCTION
+ cf_client._send_to_commander(
+ data.motorCmd1,
+ data.motorCmd2,
+ data.motorCmd3,
+ data.motorCmd4,
+ data.xControllerMode,
+ x_controller_setpoint,
+ data.yControllerMode,
+ y_controller_setpoint,
+ z_controller_mode,
+ z_controller_setpoint,
+ yaw_controller_mode,
+ yaw_controller_setpoint,
+ )
+
+# END OF: def controlCommandCallback(data):
+# ============================= #
@@ -376,20 +735,37 @@ if __name__ == '__main__':
ros_namespace = rospy.get_namespace()
- # Initialize the low-level drivers (don't list the debug drivers)
+
+ # Initialize the low-level drivers
+ # > with the option set to NOT list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
- #wait until address parameter is set by FlyingAgentClient
- while not rospy.has_param("~crazyFlieAddress"):
+ # Wait until address parameter is set by FlyingAgentClient
+ while not rospy.has_param("~crazyflieAddress"):
+ time.sleep(0.05)
+
+ # Wait until name parameter is set by FlyingAgentClient
+ # > Should be set at the same timie as the address
+ while not rospy.has_param("~crazyflieName"):
time.sleep(0.05)
- #use this following two lines to connect without data from CentralManager
+ # Use the following lines of code to connect without data
+ # from the "Central Manager":
# radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded")
# Fetch the YAML paramter "battery polling period"
global battery_polling_period
- battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
+ battery_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioCopyOfBatteryMonitor/battery_polling_period")
+
+ global cfStateEstimate_polling_period
+ cfStateEstimate_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/cfStateEstimate_polling_period")
+
+ global isEnabled_strictSafety
+ isEnabled_strictSafety = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/isEnabled_strictSafety")
+
+ global maxHeight_for_strictSafety_meters
+ maxHeight_for_strictSafety_meters = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/maxHeight_for_strictSafety_meters")
# Fetch the YAML paramter "agentID" and "coordID"
global m_agentID
@@ -398,18 +774,19 @@ if __name__ == '__main__':
# Convert the coordinator ID to a zero-padded string
coordID_as_string = format(coordID, '03')
-
# Initialise a publisher for the battery voltage
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
+ # Initialise a publisher for the state estimate
+ global cfstateEstimate_pub
+ cfstateEstimate_pub = rospy.Publisher(node_name + '/CFStateEstimate', FlyingVehicleState, queue_size=10)
+
# Initialise a "CrazyRadioClient" type variable that handles
# all communication over the CrazyRadio
global cf_client
cf_client = CrazyRadioClient()
- print "[CRAZY RADIO] DEBUG 2"
-
# Subscribe to the commands for when to (dis-)connect the
# CrazyRadio connection with the Crazyflie
# > For the radio commands from the FlyingAgentClient of this agent
@@ -420,30 +797,204 @@ if __name__ == '__main__':
# Advertise a Serice for the current status
# of the Crazy Radio connect
- s = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback)
+ getCurrentCrazyRadioStatusService = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback)
+ # Subscribe to the "flying state" of the FlyingAgentClient
+ # > This is used to determine when to reset the Crazyflie's
+ # onboard Kalman filter
+ rospy.Subscriber("FlyingAgentClient/FlyingState", Int32, cf_client.flyingStateCallback)
+ # Sleep briefly to allow everything to start-up
time.sleep(1.0)
+
+
+ # Subscribe to the "control commands" that are to be sent
+ # to the Crazyflie, i.e., commands of motor thrust and
+ # setpoints for the onboard controllers
rospy.Subscriber("FlyingAgentClient/ControlCommand", ControlCommand, controlCommandCallback)
+
+
+ # Print out some information to the user.
print "[CRAZY RADIO] Node READY :-)"
+
+ # Enter an endless while loop to keep the node alive.
rospy.spin()
+
+
+
+ # SHUT DOWN THE NODE IF THE "ros::spin" IS CANCELLED
+
+ # Inform the user
rospy.loginfo("[CRAZY RADIO] Turning off crazyflie")
- cf_client._send_to_commander_motor(0, 0, 0, 0)
- # change state to motors OFF
+ # Send the motors off command
+ cf_client._send_to_commander(0, 0, 0, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0, CF_ONBOARD_CONTROLLER_MODE_OFF, 0)
+
+ # Send a "Motors OFF" on behalf of the "FlyingAgentClient"
+ # > This serves the purpose of changing the flying state
+ # to the "Motors OFF" state
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
- cf_client.FlyingAgentClient_command_pub.publish(msg)
- #wait for client to send its commands
+ cf_client.flyingAgentClient_command_publisher.publish(msg)
+
+ # Allow some time for the command to be sent
time.sleep(1.0)
- bag.close()
- rospy.loginfo("[CRAZY RADIO] bag closed")
+ # Close the ROS loggin bag
+ #bag.close()
+ #rospy.loginfo("[CRAZY RADIO] Logging bag closed")
+
+ # Close the CrazyRadio link
cf_client._cf.close_link()
rospy.loginfo("[CRAZY RADIO] Link closed")
+
+
+# ===================================== #
+# HINTS FOR THE FORMAT STRING USED IN
+# THE "struct.pack(...)" COMMANDS FOR
+# CONSTRUCTING THE DATA PACKAGES SENT
+# TO THE CRAZYFLIE
+#
+# COPIED FROM:
+# https://docs.python.org/3/library/struct.html#format-characters
+#
+# FORMAT CHARACTERS:
+# Format characters have the following meaning; the conversion
+# between C and Python values should be obvious given their
+# types. The ‘Standard size’ column refers to the size of the
+# packed value in bytes when using standard size; that is, when
+# the format string starts with one of '<', '>', '!' or '='.
+# When using native size, the size of the packed value is
+# platform-dependent.
+#
+# |----------------------------------------------------------|
+# | Format | C Type | Python type | Size | Notes |
+# |--------------------------------------------------|-------|
+# | x | pad byte | no value | 1 | |
+# | c | char | bytes len 1 | 1 | |
+# | b | signed char | integer | 1 | (1)(2)|
+# | B | unsigned char | integer | 1 | (2) |
+# | ? | _Bool | bool | 1 | (1) |
+# | h | short | integer | 2 | (2) |
+# | H | unsigned short | integer | 2 | (2) |
+# | i | int | integer | 4 | (2) |
+# | I | unsigned int | integer | 4 | (2) |
+# | l | long | integer | 4 | (2) |
+# | L | unsigned long | integer | 4 | (2) |
+# | q | long long | integer | 8 | (2) |
+# | Q | unsigned long long | integer | 8 | (2) |
+# | n | ssize_t | integer | | (3) |
+# | N | size_t | integer | | (3) |
+# | e | (6) | float | 2 | (4) |
+# | f | float | float | 4 | (4) |
+# | d | double | float | 8 | (4) |
+# | s | char[] | bytes | | |
+# | p | char[] | bytes | | |
+# | P | void * | integer | | (5) |
+# |--------------------------------------------------|-------|
+#
+# Changed in version 3.3: Added support for the 'n'
+# and 'N' formats.
+#
+# Changed in version 3.6: Added support for the 'e'
+# format.
+#
+# Notes:
+#
+# (1) The '?' conversion code corresponds to the _Bool
+# type defined by C99. If this type is not available,
+# it is simulated using a char. In standard mode, it
+# is always represented by one byte.
+#
+# (2) When attempting to pack a non-integer using any of
+# the integer conversion codes, if the non-integer has
+# a __index__() method then that method is called to
+# convert the argument to an integer before packing.
+#
+# Changed in version 3.2: Use of the __index__() method
+# for non-integers is new in 3.2.
+#
+# (3) The 'n' and 'N' conversion codes are only available
+# for the native size (selected as the default or with
+# the '@' byte order character). For the standard size,
+# you can use whichever of the other integer formats
+# fits your application.
+#
+# (4) For the 'f', 'd' and 'e' conversion codes, the packed
+# representation uses the IEEE 754 binary32, binary64 or
+# binary16 format (for 'f', 'd' or 'e' respectively),
+# regardless of the floating-point format used by the
+# platform.
+#
+# (5) The 'P' format character is only available for the
+# native byte ordering (selected as the default or with
+# the '@' byte order character). The byte order character
+# '=' chooses to use little- or big-endian ordering based
+# on the host system. The struct module does not interpret
+# this as native ordering, so the 'P' format is not
+# available.
+#
+# (6) The IEEE 754 binary16 “half precision†type was
+# introduced in the 2008 revision of the IEEE 754 standard.
+# It has a sign bit, a 5-bit exponent and 11-bit precision
+# (with 10 bits explicitly stored), and can represent
+# numbers between approximately 6.1e-05 and 6.5e+04 at
+# full precision. This type is not widely supported by C
+# compilers: on a typical machine, an unsigned short can
+# be used for storage, but not for math operations. See
+# the Wikipedia page on the half-precision floating-point
+# format for more information.
+#
+# A format character may be preceded by an integral repeat
+# count. For example, the format string '4h' means exactly the
+# same as 'hhhh'.
+#
+# Whitespace characters between formats are ignored; a count
+# and its format must not contain whitespace though.
+#
+# For the 's' format character, the count is interpreted as
+# the length of the bytes, not a repeat count like for the
+# other format characters; for example, '10s' means a single
+# 10-byte string, while '10c' means 10 characters. If a count
+# is not given, it defaults to 1. For packing, the string is
+# truncated or padded with null bytes as appropriate to make
+# it fit. For unpacking, the resulting bytes object always has
+# exactly the specified number of bytes. As a special case,
+# '0s' means a single, empty string (while '0c' means 0
+# characters).
+#
+# When packing a value x using one of the integer formats
+# ('b', 'B', 'h', 'H', 'i', 'I', 'l', 'L', 'q', 'Q'), if x is
+# outside the valid range for that format then struct.error is
+# raised.
+#
+# Changed in version 3.1: In 3.0, some of the integer formats
+# wrapped out-of-range values and raised DeprecationWarning
+# instead of struct.error.
+#
+# The 'p' format character encodes a “Pascal stringâ€, meaning
+# a short variable-length string stored in a fixed number of
+# bytes, given by the count. The first byte stored is the
+# length of the string, or 255, whichever is smaller. The
+# bytes of the string follow. If the string passed in to
+# pack() is too long (longer than the count minus 1), only
+# the leading count-1 bytes of the string are stored. If the
+# string is shorter than count-1, it is padded with null bytes
+# so that exactly count bytes in all are used. Note that for
+# unpack(), the 'p' format character consumes count bytes,
+# but that the string returned can never contain more than
+# 255 bytes.
+
+# For the '?' format character, the return value is either
+# True or False. When packing, the truth value of the argument
+# object is used. Either 0 or 1 in the native or standard bool
+# representation will be packed, and any non-zero value will
+# be True when unpacking.
+#
+# ===================================== #
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
index 9598f8522e074e78f73cda20bb755eeb318cdf9e..0f3431c9e9cf2f87033f1ad84275a809057ba8e5 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
@@ -210,7 +210,7 @@ if __name__ == '__main__':
cflib.crtp.init_drivers(enable_debug_driver=False)
#wait until address parameter is set by FlyingAgentClient
- # while not rospy.has_param("~crazyFlieAddress"):
+ # while not rospy.has_param("~crazyflieAddress"):
# time.sleep(0.05)
#use this following two lines to connect without data from CentralManager
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CrazyflieIO.h b/dfall_ws/src/dfall_pkg/include/classes/CrazyflieIO.h
similarity index 100%
rename from dfall_ws/src/dfall_pkg/include/nodes/CrazyflieIO.h
rename to dfall_ws/src/dfall_pkg/include/classes/CrazyflieIO.h
diff --git a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
index 4174f8f453d5d4c22f40c0916735982e6b407f06..fb35125283a24eb5aef812eb98eee073c3031b74 100644
--- a/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
+++ b/dfall_ws/src/dfall_pkg/include/classes/GetParamtersAndNamespaces.h
@@ -107,15 +107,19 @@ using namespace dfall_pkg;
// GET YAML PARAMETER FUNCTIONS
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterStringVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val, int length);
+int getParameterStringVectorUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val);
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterFloatVectorUnnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val);
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);
-std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
@@ -131,4 +135,7 @@ void constructNamespaceForCoordinator( int coordID, std::string & coord_namespac
void constructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace );
// Check the header of a message for whether it is relevant
-bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::vector<uint> & agentIDs );
\ No newline at end of file
+bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::vector<uint> & agentIDs );
+
+// FUNCTION FOR CONVERTING NEWTON TO MOTOR COMMAND
+float newtons2cmd_for_crazyflie( float thrust);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h b/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h
index b520e0f052f48fb48e0bc4c83ad78d13fb885801..ec9de4ce6ba7973ca94a197a5f8db0e95c24cd46 100644
--- a/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h
+++ b/dfall_ws/src/dfall_pkg/include/classes/QuadrotorSimulator.h
@@ -101,6 +101,9 @@ class QuadrotorSimulator
public:
+ // Empty Constructor
+ QuadrotorSimulator ();
+
// Default Constructor
QuadrotorSimulator ( std::string id );
@@ -111,6 +114,15 @@ public:
public:
+ // 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:
+
// Identifier String
std::string m_id_string = "none";
@@ -120,16 +132,6 @@ public:
// 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;
@@ -194,6 +196,21 @@ private:
public:
+ // Function to get the ID as a string
+ std::string get_id_string();
+
+ // Function to set the ID as a string
+ void set_id_string(std::string new_id_string);
+
+ // Function to get the mass in kilograms
+ float get_mass_in_kg();
+
+ // Function to set the mass in kilograms
+ void set_mass_in_kg(float new_mass_in_kg);
+
+ // Function to get the flying state
+ bool get_isFlying();
+
// Function to simulate the quadrotor for one time step
void simulate_for_one_time_step(float deltaT);
@@ -203,8 +220,7 @@ public:
// 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);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/AgentStatusForWebInterface.h b/dfall_ws/src/dfall_pkg/include/nodes/AgentStatusForWebInterface.h
new file mode 100644
index 0000000000000000000000000000000000000000..c7dcef7ef98362f9bd8875a3c153612c31c96fb8
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/AgentStatusForWebInterface.h
@@ -0,0 +1,166 @@
+// Copyright (C) 2020, The University of Melbourne, Department of Electrical and Electronic Engineering (EEE), 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 provides data to the web interface
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 <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/SetpointWithHeader.h"
+#include "dfall_pkg/FlyingVehicleState.h"
+#include "dfall_pkg/DebugMsg.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/IntStringService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+//#include "classes/GetParamtersAndNamespaces.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
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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 crazyradio status
+// > as received via messages
+int m_crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
+
+// The battery level
+// > as received via messages
+int m_battery_level = BATTERY_LEVEL_UNAVAILABLE;
+
+// The flying state of the agent
+// > as received via messages
+int m_agent_operating_state = STATE_UNAVAILABLE;
+
+// The instant controller,
+// > as received via messages
+int m_instant_controller = DEFAULT_CONTROLLER;
+
+// The setpoint of the default controller
+// > as received via messages
+float m_setpoint_default[4] = {0.0,0.0,0.4,0.0};
+
+// The setpoint of the student controller
+// > as received via messages
+float m_setpoint_student[4] = {0.0,0.0,0.4,0.0};
+
+// The debug values of the student controller
+// > as received via messages
+float m_debug_values_student[10] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+
+// The current state estimate of the Crazyflie
+// > as received via messages
+float m_cf_state_estimate_xyz_rpy[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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
+// ----------------------------------------------------------------------------------
+
+
+// Callbacks for messages with the status of things:
+// > For the status of the crazyradio
+void crazyRadioStatusCallback(const std_msgs::Int32& msg);
+// > For the battery level
+void newBatteryLevelCallback(const std_msgs::Int32& msg);
+// > For the flying state of the agent
+void agentOperatingStateCallback(const std_msgs::Int32& msg);
+// > For the instant controller
+void instantControllerChangedCallback(const std_msgs::Int32& msg);
+// > For the Default Controller Setpoint
+void defaultControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint);
+// > For the Student Controller Setpoint
+void studentControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint);
+// > For the Student Controller Debug Values
+void studentControllerDebugValuesCallback(const DebugMsg& newDebugMsg);
+// > For the State Estimate from the Crazyflie
+void cfStateEstimateCallback(const FlyingVehicleState & newStateEstimate);
+
+// Service callback for providing status to the Web Interface
+bool statusForWebInterfaceCallback(IntStringService::Request &request, IntStringService::Response &response);
\ 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 8d6f536eda2812023ce1912a623a1ce5f9df471e..cd2abf23cab94473518b1ffcc6395ee8d280ae91 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CentralManagerService.h
@@ -61,7 +61,7 @@
#include "dfall_pkg/CMCommand.h"
// Include other classes
-#include "CrazyflieIO.h"
+#include "classes/CrazyflieIO.h"
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index 7c4b3dd4d687b629f6fb14a9902a4717a8aa4d49..4bd6a0942eecd09498cf4c702ac66fc8ddc93eaa 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
@@ -41,7 +41,6 @@
// UUU
// ----------------------------------------------------------------------------------
-
// Conversions between degrees and radians
#define RAD2DEG 180.0/PI
#define DEG2RAD PI/180.0
@@ -64,8 +63,6 @@
//
// ----------------------------------------------------------------------------------
-
-
// The types, i.e., agent or coordinator
#define TYPE_INVALID -1
#define TYPE_COORDINATOR 1
@@ -111,6 +108,31 @@
#define CF_COMMAND_TYPE_ANGLE 10
+#define CF_ONBOARD_CONTROLLER_MODE_OFF 0
+#define CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE 1
+#define CF_ONBOARD_CONTROLLER_MODE_ANGLE 2
+#define CF_ONBOARD_CONTROLLER_MODE_VELOCITY 3
+#define CF_ONBOARD_CONTROLLER_MODE_POSITION 4
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// Types of flying vehicle states:
+#define FLYING_VEHICLE_STATE_TYPE_NONE 0
+#define FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT 1
+#define FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE 2
+#define FLYING_VEHICLE_STATE_TYPE_12_STATE_ESTIMATE 3
+
+
@@ -175,6 +197,7 @@
+
// ----------------------------------------------------------------------------------
// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
// B B A A T T E R R Y Y
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
index 03905238f5f1c6bcf7e760b8b76832e56306616f..a4af6736fc89346d2ec3a7a6aad2ff25b130ee60 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
@@ -57,13 +57,17 @@
// Include the DFALL message types
#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/ControlCommand.h"
+#include "dfall_pkg/FlyingVehicleState.h"
// Include the DFALL service types
#include "dfall_pkg/IntIntService.h"
// Include the shared definitions
#include "nodes/Constants.h"
+
+// Include other classes
#include "classes/GetParamtersAndNamespaces.h"
+#include "classes/QuadrotorSimulator.h"
// SPECIFY THE PACKAGE NAMESPACE
using namespace dfall_pkg;
@@ -112,6 +116,9 @@ int m_radio_state = CRAZY_RADIO_STATE_DISCONNECTED;
// Timer for the battery voltage updates
ros::Timer m_timer_battery_voltage_updates;
+// Timer for the state estimte updates
+ros::Timer m_timer_state_estimate_update;
+
// PUBLISHERS
@@ -121,16 +128,21 @@ ros::Publisher crazyRadioStatusPublisher;
// Publisher for sending a "Flying Agent Client Command"
ros::Publisher flyingAgentClientCommandPublisher;
-// Publisher for the filtered battery voltage
+// Publisher for the raw battery voltage
ros::Publisher batteryVoltagePublisher;
+// Publisher for the onboard state estimate
+ros::Publisher cfStateEstimatorPublisher;
+
// Publisher for the control commands
// > Note this is not needed for the real CrazyRadio node because
// this command goes out over the radio, but for emulation we publish
// the control command
ros::Publisher controlCommandPublisher;
-
+// Publisher for the simulation state
+// > This is used by the MocapEmulator node
+ros::Publisher cfSimulationStatePublisher;
@@ -141,6 +153,11 @@ ros::Publisher controlCommandPublisher;
// Frequency of requesting the battery voltage, in [seconds]
float yaml_battery_polling_period_in_seconds = 0.2f;
+// Flag for whether to use height as a trigger for
+// publishing a motors-OFF command
+bool yaml_isEnabled_strictSafety = true;
+float yaml_maxHeight_for_strictSafety_meters = 1.2;
+
// // Battery thresholds while in the "motors off" state, in [Volts]
// float yaml_battery_voltage_threshold_lower_while_standby = 3.30f;
// float yaml_battery_voltage_threshold_upper_while_standby = 4.20f;
@@ -155,6 +172,36 @@ float yaml_battery_polling_period_in_seconds = 0.2f;
+
+
+// VARIABLES FOR SIMULATING A QUAROTOR
+
+// A QUADROTORS SIMULATOR
+QuadrotorSimulator m_quadrotor_sim;
+
+// SIMULATION FREQUENCY FOR EMULATING A CRAZYFLIE
+//float yaml_cfStateEstimate_polling_period_in_seconds = 0.02f;
+float yaml_cfSimulation_frequency = 200.0;
+float yaml_cfSimulation_deltaT_in_seconds = 0.005;
+
+// HOW OFTEN TO PUBLISH THE SIMULATION STATE AS AN
+// ONBOARD ESTIMATE
+// > i.e., this integer divided by the
+// "cfSimulation_frequency" gives the simulation
+// equivalent of "cfStateEstimate_polling_period"
+int yaml_cfSimulation_stateEstimate_sendEvery = 4;
+
+// 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.0f;
+float yaml_command_sixteenbit_max = 65000.0f;
+
+
+
+
+
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
@@ -195,8 +242,12 @@ bool getCurrentCrazyRadioStatusServiceCallback(IntIntService::Request &request,
// UPDATE BATTERY VOLTAGE TIMER CALLBACK
-void update_battery_voltage_callback(const ros::TimerEvent&);
+void timerCallback_update_battery_voltage(const ros::TimerEvent&);
+
+// UPDATE STATE ESTIMATE TIMER CALLBACK
+void timerCallback_update_cfStateEstimate(const ros::TimerEvent&);
// LOAD YAML PARAMETER FUNCTIONS
void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg);
-void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
+void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle);
+void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index a838f2f2b2a8b8ca5ea03ede4742ae9f56b9c5f3..bb7a47f7d1f0f5bb61fb1d492fcef401ead9901e 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -44,7 +44,7 @@
// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.
-//some useful libraries
+// Include useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
@@ -329,7 +329,7 @@ float yaml_integratorGain_forTauYaw = 0.0;
// VARIABLES FOR THE ESTIMATOR
// Frequency at which the controller is running
-float m_estimator_frequency = 200.0;
+float yaml_estimator_frequency = 200.0;
// > A flag for which estimator to use:
int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
@@ -387,7 +387,9 @@ 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;
+//float m_symmetric_area_bounds_z = 0.5;
+float m_area_bounds_zmin = 0.0;
+float m_area_bounds_zmax = 1.0;
// Service Client from which the "CrazyflieContext" is loaded
ros::ServiceClient m_centralManager;
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h
index 111c7e4722b7253a962047a78d08a677233de335..a4791592b551052f5642c0d1a3d81b7ce338b2be 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DemoControllerService.h
@@ -378,21 +378,6 @@ ros::Publisher my_current_xyz_yaw_publisher;
ros::Subscriber xyz_yaw_to_follow_subscriber;
-// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
-// The "CrazyflieData" type used for the "request" variable is a
-// structure as defined in the file "CrazyflieData.msg" which has the following
-// properties:
-// string crazyflieName The name given to the Crazyflie in the Vicon software
-// float64 x The x position of the Crazyflie [metres]
-// float64 y The y position of the Crazyflie [metres]
-// float64 z The z position of the Crazyflie [metres]
-// float64 roll The roll component of the intrinsic Euler angles [radians]
-// float64 pitch The pitch component of the intrinsic Euler angles [radians]
-// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
-
-
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index 61b6d33059caac567b9d635401da2ecb23fd3a4f..5e09499b58d66bf629711c54b55a296f47f249b9 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -55,7 +55,7 @@
// Include the DFALL message types
#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/ViconData.h"
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/CrazyflieContext.h"
#include "dfall_pkg/Setpoint.h"
@@ -124,30 +124,46 @@ std::string m_namespace_to_coordinator_parameter_service;
+// VARIABLES FOR SOTRING THE PARAMETERS OF WHICH
+// MEASUREMENT STREAMS TO USE
+bool yaml_shouldUse_mocapMeasurements = false;
+bool yaml_shouldUse_onboardEstimate = false;
+
+// SUBSCRIBERS FOR THE MEASUREMENT STREAMS
+ros::Subscriber viconSubscriber;
+ros::Subscriber onboardEstimateSubscriber;
+
+
+
+// VARIABLES FOR AVAILABILITY AND VALIDITY OF MEASUREMENTS
+// > Boolen indicating if the Mocap data is availble
+bool m_isAvailable_measurement_data = false;
+// > Boolen indicating if the object is "long term" invalid
+bool m_isValid_measurement_data = false;
+// > Number of consecutive invalid measurements that triggers
+// the "m_isValid_measurement_data" variable to be "false"
+int yaml_consecutive_invalid_threshold = 30;
+// > Timer that when triggered determines that the
+// "m_isAvailable_measurement_data" variable becomes false
+ros::Timer m_timer_measurement_data_timeout_check;
+// > Time out duration after which measurement data is
+// considered unavailable
+float yaml_measurement_timeout_duration = 1.0;
+
+
+
// VARIABLES FOR THE MOTION CAPTURE DATA
// > The index for which element in the "motion capture
// data" array is expected to match the name in
// "m_context", (negative numbers indicate unknown)
int m_poseDataIndex = -1;
-// > Boolen indicating if the Mocap data is availble
-bool m_isAvailable_mocap_data = false;
-// > Boolen indicating if the object is "long term" occuled
-bool m_isOcculded_mocap_data = false;
-// > Number of consecutive occulsions that trigger the
-// "m_isOcculded_mocap_data" variable to be "true"
-int yaml_consecutive_occulsions_threshold = 30;
-// > Timer that when triggered determines that the
-// "m_isAvailable_mocap_data" variable becomes true
-ros::Timer m_timer_mocap_timeout_check;
-// > Time out duration after which Mocap is considered unavailable
-float yaml_mocap_timeout_duration = 1.0;
-
-// VARIABLES FOR GETTING MOTION CAPTURE DATA OF OTHER OBJECTS
+// > The index for getting motion capture data of other
+// objects
int m_otherObjectPoseDataIndex = -1;
-// VARIABLES FOR STORING THE PARAMTER OF THE POSITION
+// VARIABLES FOR STORING THE PARAMETERS FOR THE POSITION
// AND TILT ANGLE SAFTY CHECKS
// > Boolean indicating whether the tilt angle check
// should be performed
@@ -276,21 +292,36 @@ ros::Publisher m_controllerUsedPublisher;
void viconCallback(const ViconData& viconData);
// > For extracting the pose data of an specific
// object by name
-int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index ,CrazyflieData& pose);
+int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index ,FlyingVehicleState& pose);
// > For converting the global frame motion capture
// data to the local frame of this agent
-void coordinatesToLocal(CrazyflieData& cf);
-// > Callback run when motion capture data has not
-// been receive for a specified time
-void timerCallback_mocap_timeout_check(const ros::TimerEvent&);
+void coordinatesToLocal(FlyingVehicleState& cf);
+
+
+
+// FUNCTIONS FOR HANDLING THE ONBOARD STATE ESTIMATE
+// > Callback run every time new state estimate
+// data is available from onboard the flying vehicle
+void onboardEstimateCallback(const FlyingVehicleState& flyingVehicleState);
+
+
+
+// FUNCTIONS FOR CALLING THE CONTROLLER SERVICE
+// > For calling the controller service for the currently
+// selected controller, and then sending the control
+// command to the CrazyRadio node
+void callControllerServiceWithGivenData(FlyingVehicleState& dataForThisAgent, FlyingVehicleState& dataForOtherAgent);
// > For sending a command, via the CrazyRadio, that
// the motors should be set to zero
void sendZeroOutputCommandForMotors();
+// > Callback run when measurement data has not
+// been received for a specified time
+void timerCallback_measurement_data_timeout_check(const ros::TimerEvent&);
// FOR THE SAFETY CHECKS ON POSITION AND TILT ANGLE
-bool safetyCheck_on_positionAndTilt(CrazyflieData data);
+bool safetyCheck_on_positionAndTilt(FlyingVehicleState data);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
index ec94b908c16bd2c1ab25f188b2d66b9c90dc9c17..d73349f7da1e55dba4009506e239c6f103a57122 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
@@ -55,7 +55,7 @@
// Include the DFALL message types
#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/ViconData.h"
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/CrazyflieContext.h"
#include "dfall_pkg/AreaBounds.h"
@@ -129,6 +129,11 @@ float yaml_command_sixteenbit_max = 65000;
// A VECTOR OF QUADROTORS
std::vector<QuadrotorSimulator> m_quadrotor_fleet;
+// A VECTOR OF FLYING VEHICLE STATES
+std::vector<FlyingVehicleState> m_flying_fleet_states;
+std::vector<ros::Subscriber> m_flying_fleet_state_subscribers;
+std::vector<AreaBounds> m_flying_fleet_areaBounds;
+
// PUBLISHER FOR THE MOTION CAPTURE DATA
ros::Publisher m_mocapDataPublisher;
@@ -153,6 +158,10 @@ ros::ServiceClient m_centralManagerService;
// CALLBACK FOR EVERYTIME MOCAP DATA SHOULD BE PUBLISHED
void timerCallback_mocap_publisher(const ros::TimerEvent&);
+// CALLBACK FOR RECEIVEING CRAZYFLIE SIMULATION DATA
+// > This data is published by the CrazyRadioEmulator node
+void cfSimulationStateCallback(const FlyingVehicleState & msg);
+
// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
// > Callback that the context database changed
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h b/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
index d298e9e2b2456acf03f5aac57ed73ba69d341ee5..bed209750ff56561b977b3f6568dd38f94210bea 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/ParameterService.h
@@ -43,6 +43,7 @@
#include <stdlib.h>
#include <string>
+#include <map>
#include <ros/ros.h>
#include <ros/package.h>
@@ -65,6 +66,7 @@
#include "nodes/Constants.h"
+
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
// D D E F I NN N E S
@@ -106,8 +108,15 @@ std::string m_base_namespace;
// Publisher for passing a service request onto the
// loadinging function
-ros::Publisher requestLoadYamlFilenamePublisher;
+ros::Publisher m_requestLoadYamlFilenamePublisher;
+
+// Vector of strings loaded from YAML file with the
+// file names of yaml files to load
+std::vector<std::string> yaml_filenames_provided;
+// Map of Publishers for informing the network that this
+// Parameter service node loaded a requested YAML file
+std::map<std::string, ros::Publisher> m_yamlParametersReadyForFetchPublisherMap;
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
index a47c52b54922052e5da14b3eb3656cabaeba1d18..75b3273b46b541ef7ac05df07edbffdd45655d9d 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/RemoteControllerService.h
@@ -109,26 +109,6 @@ using namespace dfall_pkg;
// These constants are defined to make the code more readable and adaptable.
-// These constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
-// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-
-// #define CF_COMMAND_TYPE_MOTOR 6
-// #define CF_COMMAND_TYPE_RATE 7
-// #define CF_COMMAND_TYPE_ANGLE 8
-
-
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
// The following is a short description about each mode:
@@ -398,23 +378,6 @@ int m_coordID;
-// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
-// The "CrazyflieData" type used for the "request" variable is a
-// structure as defined in the file "CrazyflieData.msg" which has the following
-// properties:
-// string crazyflieName The name given to the Crazyflie in the Vicon software
-// float64 x The x position of the Crazyflie [metres]
-// float64 y The y position of the Crazyflie [metres]
-// float64 z The z position of the Crazyflie [metres]
-// float64 roll The roll component of the intrinsic Euler angles [radians]
-// float64 pitch The pitch component of the intrinsic Euler angles [radians]
-// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
-
-
-
-
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
index 3a540f297544dfa279d053e34d8265c3e606ba61..2fa26a998ab4be3277d583b2256a126972fa9508 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/SafeControllerService.h
@@ -56,7 +56,7 @@
// Include the DFALL message types
#include "dfall_pkg/IntWithHeader.h"
-#include "dfall_pkg/CrazyflieData.h"
+#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/Setpoint.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/Controller.h"
@@ -155,7 +155,7 @@ std::vector<float> setpoint(4);
std::vector<float> defaultSetpoint(4);
float saturationThrust;
-CrazyflieData previousLocation;
+FlyingVehicleState previousLocation;
// Variable for the namespaces for the paramter services
@@ -199,7 +199,7 @@ void processFetchedParameters();
// > For the GETPARAM()
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+void getParameterFloatVectorKnownLength(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);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
index 91d6b3e89d67aec767d7c5cf8fe3a740a16a8497..bb04171a063e7ee268acf96ec43333676aaddcb1 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/StudentControllerService.h
@@ -69,6 +69,7 @@
// Include the DFALL service types
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
+//#include "dfall_pkg/GetDebugValuesService.h"
// Include the shared definitions
#include "nodes/Constants.h"
@@ -167,10 +168,6 @@ float yaml_cf_mass_in_grams = 25.0;
// The weight of the Crazyflie in Newtons, i.e., mg
float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81 / 1000.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 frequency at which the controller is running
float yaml_control_frequency = 200.0;
@@ -236,9 +233,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// 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);
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h
index bfe8717bd41138ae6754cba3bd40951ade020260..3619494211d700b273e9b6d19a2c983ca2f5f54c 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TestMotorsControllerService.h
@@ -123,68 +123,6 @@ 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;
-
@@ -211,27 +149,4 @@ ros::Publisher m_setpointChangedPublisher;
// 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
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
\ 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 5534b1baa30f246f1f12df48770bcb77b184bc5f..cff8cd0ce3a893114031ecd0f50cfac2f18566a4 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TuningControllerService.h
@@ -112,26 +112,6 @@
// These constants are defined to make the code more readable and adaptable.
-// These constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
-// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-
-#define CF_COMMAND_TYPE_MOTOR 6
-#define CF_COMMAND_TYPE_RATE 7
-#define CF_COMMAND_TYPE_ANGLE 8
-
-
// These constants define the controller used for computing the response in the
// "calculateControlOutput" function
// The following is a short description about each mode:
@@ -436,23 +416,6 @@ int m_coordID;
-// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
-// The "CrazyflieData" type used for the "request" variable is a
-// structure as defined in the file "CrazyflieData.msg" which has the following
-// properties:
-// string crazyflieName The name given to the Crazyflie in the Vicon software
-// float64 x The x position of the Crazyflie [metres]
-// float64 y The y position of the Crazyflie [metres]
-// float64 z The z position of the Crazyflie [metres]
-// float64 roll The roll component of the intrinsic Euler angles [radians]
-// float64 pitch The pitch component of the intrinsic Euler angles [radians]
-// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
-
-
-
-
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index cccd6020e3fc39aedd190ba90a19c3bdce133683..0ed62bb46297b6ae7a1b30cb2974fcc45f5bd8bc 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/dfall_ws/src/dfall_pkg/launch/agent.launch
@@ -31,7 +31,16 @@
output = "screen"
type = "CrazyRadio.py"
>
- <rosparam command="load" file="$(find dfall_pkg)/param/BatteryMonitor.yaml" />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/BatteryMonitor.yaml"
+ ns = "CrazyRadioCopyOfBatteryMonitor"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/CrazyRadioConfig.yaml"
+ ns = "CrazyRadioConfig"
+ />
</node>
</group>
<group if="$(arg emulateRadio)">
@@ -41,7 +50,11 @@
output = "screen"
type = "CrazyRadioEmulator"
>
- <rosparam command="load" file="$(find dfall_pkg)/param/BatteryMonitor.yaml" />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/CrazyRadioConfig.yaml"
+ ns = "CrazyRadioConfig"
+ />
</node>
</group>
@@ -147,6 +160,11 @@
>
<param name="type" type="str" value="agent" />
<param name="agentID" value="$(arg agentID)" />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/YamlFileNames.yaml"
+ ns = "YamlFileNames"
+ />
<rosparam
command = "load"
file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
@@ -159,28 +177,13 @@
/>
<rosparam
command = "load"
- file = "$(find dfall_pkg)/param/SafeController.yaml"
- ns = "SafeController"
- />
- <rosparam
- command = "load"
- file = "$(find dfall_pkg)/param/RemoteController.yaml"
- ns = "RemoteController"
- />
- <rosparam
- command = "load"
- file = "$(find dfall_pkg)/param/TuningController.yaml"
- ns = "TuningController"
+ file = "$(find dfall_pkg)/param/DefaultController.yaml"
+ ns = "DefaultController"
/>
<rosparam
command = "load"
- file = "$(find dfall_pkg)/param/PickerController.yaml"
- ns = "PickerController"
- />
- <rosparam
- command = "load"
- file = "$(find dfall_pkg)/param/TemplateController.yaml"
- ns = "TemplateController"
+ file = "$(find dfall_pkg)/param/StudentController.yaml"
+ ns = "StudentController"
/>
</node>
@@ -193,10 +196,36 @@
output = "screen"
type = "FlyingAgentGUI"
>
- <param name="type" type="str" value="agent" />
- <param name="agentID" value="$(arg agentID)" />
+ <param
+ name="type"
+ type="str"
+ value="agent"
+ />
+ <param
+ name="agentID"
+ value="$(arg agentID)"
+ />
+ <param
+ name="onlaunch_shouldEmitPoseDataMocap"
+ type="bool"
+ value="true"
+ />
+ <param
+ name="onlaunch_shouldEmitPoseDataOnboard"
+ type="bool"
+ value="false"
+ />
</node>
</group>
+
+ <!-- AGENT STATUS FOR WEB INTERFACE -->
+ <node
+ pkg = "dfall_pkg"
+ name = "AgentStatusForWebInterface"
+ output = "screen"
+ type = "AgentStatusForWebInterface"
+ >
+ </node>
</group>
diff --git a/dfall_ws/src/dfall_pkg/launch/pi_agent.launch b/dfall_ws/src/dfall_pkg/launch/pi_agent.launch
new file mode 100755
index 0000000000000000000000000000000000000000..e54ae35197193d785326dce1ac3a01ab49bda64e
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/launch/pi_agent.launch
@@ -0,0 +1,153 @@
+<launch>
+
+ <!-- INPUT ARGUMENT OF THE AGENT's ID -->
+ <arg name="agentID" default="$(optenv DFALL_DEFAULT_AGENT_ID)" />
+
+ <!-- INPUT ARGUMENT OF THE COORDINATOR's ID -->
+ <arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
+
+ <!-- INPUT ARGUMENT FOR EMULATING THE CRAZY RADIO -->
+ <arg name="emulateRadio" default="false" />
+
+ <!-- Example of how to use the value in agentID -->
+ <!-- <param name="param" value="$(arg agentID)"/> -->
+
+ <!-- Example of how to specify the agentID from command line -->
+ <!-- roslaunch dfall_pkg agentID:=1 -->
+
+ <!-- Example of how to specify the withGUI from command line -->
+ <!-- roslaunch dfall_pkg withGUI:=false -->
+
+ <group ns="$(eval 'agent' + str(agentID).zfill(3))">
+
+ <!-- CRAZY RADIO -->
+ <group unless="$(arg emulateRadio)">
+ <node
+ pkg = "dfall_pkg"
+ name = "CrazyRadio"
+ output = "screen"
+ type = "CrazyRadio.py"
+ >
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/BatteryMonitor.yaml"
+ ns = "CrazyRadioCopyOfBatteryMonitor"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/CrazyRadioConfig.yaml"
+ ns = "CrazyRadioConfig"
+ />
+ </node>
+ </group>
+ <group if="$(arg emulateRadio)">
+ <node
+ pkg = "dfall_pkg"
+ name = "CrazyRadio"
+ output = "screen"
+ type = "CrazyRadioEmulator"
+ >
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/CrazyRadioConfig.yaml"
+ ns = "CrazyRadioConfig"
+ />
+ </node>
+ </group>
+
+
+ <!-- FLYING AGENT CLIENT -->
+ <node
+ pkg = "dfall_pkg"
+ name = "FlyingAgentClient"
+ output = "screen"
+ type = "FlyingAgentClient"
+ >
+ <param name="agentID" value="$(arg agentID)" />
+ <param name="coordID" value="$(arg coordID)" />
+ </node>
+
+ <!-- BATTERY MONITOR -->
+ <node
+ pkg = "dfall_pkg"
+ name = "BatteryMonitor"
+ output = "screen"
+ type = "BatteryMonitor"
+ >
+ </node>
+
+ <!-- DEFAULT CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "DefaultControllerService"
+ output = "screen"
+ type = "DefaultControllerService"
+ >
+ </node>
+
+ <!-- STUDENT CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "StudentControllerService"
+ output = "screen"
+ type = "StudentControllerService"
+ >
+ </node>
+
+ <!-- TEST MOTORS CONTROLLER -->
+ <node
+ pkg = "dfall_pkg"
+ name = "TestMotorsControllerService"
+ output = "screen"
+ type = "TestMotorsControllerService"
+ >
+ </node>
+
+ <!-- PARAMETER SERVICE -->
+ <node
+ pkg = "dfall_pkg"
+ name = "ParameterService"
+ output = "screen"
+ type = "ParameterService"
+ >
+ <param name="type" type="str" value="agent" />
+ <param name="agentID" value="$(arg agentID)" />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/YamlFileNames.yaml"
+ ns = "YamlFileNames"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/FlyingAgentClientConfig.yaml"
+ ns = "FlyingAgentClientConfig"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/BatteryMonitor.yaml"
+ ns = "BatteryMonitor"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/DefaultController.yaml"
+ ns = "DefaultController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find dfall_pkg)/param/StudentController.yaml"
+ ns = "StudentController"
+ />
+ </node>
+
+ <!-- AGENT STATUS FOR WEB INTERFACE -->
+ <node
+ pkg = "dfall_pkg"
+ name = "AgentStatusForWebInterface"
+ output = "screen"
+ type = "AgentStatusForWebInterface"
+ >
+ </node>
+
+ </group>
+
+</launch>
diff --git a/dfall_ws/src/dfall_pkg/launch/pi_master.launch b/dfall_ws/src/dfall_pkg/launch/pi_master.launch
new file mode 100755
index 0000000000000000000000000000000000000000..3165b14b65989eb9a2463611e0dad1b3d25f6588
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/launch/pi_master.launch
@@ -0,0 +1,40 @@
+<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"
+ name="CentralManagerService"
+ output="screen"
+ type="CentralManagerService"
+ >
+ </node>
+
+ <!-- VICON DATA PUBLISHER -->
+ <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>
+
+</launch>
diff --git a/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg b/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg
index b367b15d881e07d6863e968894758df1abbdb421..5b82cabb545a104095e006efb460fa0d6669237f 100755
--- a/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg
+++ b/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg
@@ -4,5 +4,4 @@ float32 xmax
float32 ymin
float32 ymax
float32 zmin
-float32 zmax
-
+float32 zmax
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/ControlCommand.msg b/dfall_ws/src/dfall_pkg/msg/ControlCommand.msg
index c75b1e2c646826cdccf6ddd23cec1f0a3f98d9a5..bf56eeb25bca68b2b4d1425b7b8c780c6fca8b20 100644
--- a/dfall_ws/src/dfall_pkg/msg/ControlCommand.msg
+++ b/dfall_ws/src/dfall_pkg/msg/ControlCommand.msg
@@ -1,9 +1,12 @@
-float32 roll
-float32 pitch
-float32 yaw
uint16 motorCmd1
uint16 motorCmd2
uint16 motorCmd3
uint16 motorCmd4
-uint8 onboardControllerType
-
+uint8 xControllerMode
+uint8 yControllerMode
+uint8 zControllerMode
+uint8 yawControllerMode
+float32 xControllerSetpoint
+float32 yControllerSetpoint
+float32 zControllerSetpoint
+float32 yawControllerSetpoint
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/CrazyflieContext.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieContext.msg
index 314275670853701f06ae18deb36ac18c139c4f2a..95974964c296edfe37dd85b0f1a34b1187bac7ee 100755
--- a/dfall_ws/src/dfall_pkg/msg/CrazyflieContext.msg
+++ b/dfall_ws/src/dfall_pkg/msg/CrazyflieContext.msg
@@ -1,3 +1,3 @@
string crazyflieName
string crazyflieAddress
-AreaBounds localArea
+AreaBounds localArea
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg
index a65dc00ec0ff10d42d5f97f88d6b3add6c2693d9..28ffc012bf7c1f8ee4f190a6442dc911e7c82318 100644
--- a/dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg
+++ b/dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg
@@ -1,2 +1 @@
-CrazyflieEntry[] crazyflieEntries
-
+CrazyflieEntry[] crazyflieEntries
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/CrazyflieData.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieData.msg
deleted file mode 100644
index 91ddb357d42ceee61cc6d88f76ee6d65633197e7..0000000000000000000000000000000000000000
--- a/dfall_ws/src/dfall_pkg/msg/CrazyflieData.msg
+++ /dev/null
@@ -1,9 +0,0 @@
-string crazyflieName
-float64 x
-float64 y
-float64 z
-float64 roll
-float64 pitch
-float64 yaw
-float64 acquiringTime #delta t
-bool occluded
diff --git a/dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg
index 1bf527e228dcbd969357097a1bb37a6ff6f59af7..b8d7fd54b835c2ec23cd3e028da58bc6698767d6 100644
--- a/dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg
+++ b/dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg
@@ -1,3 +1,2 @@
CrazyflieContext crazyflieContext
-uint32 studentID
-
+uint32 studentID
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/FlyingVehicleState.msg b/dfall_ws/src/dfall_pkg/msg/FlyingVehicleState.msg
new file mode 100644
index 0000000000000000000000000000000000000000..1e0b1c4ee9781e16d257712e03d4bd833d4d68ff
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/FlyingVehicleState.msg
@@ -0,0 +1,16 @@
+uint32 type
+string vehicleName
+float64 x
+float64 y
+float64 z
+float64 vx
+float64 vy
+float64 vz
+float64 roll
+float64 pitch
+float64 yaw
+float64 rollRate
+float64 pitchRate
+float64 yawRate
+float64 acquiringTime #delta t
+bool isValid
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/GyroMeasurements.msg b/dfall_ws/src/dfall_pkg/msg/GyroMeasurements.msg
new file mode 100644
index 0000000000000000000000000000000000000000..79ec27399711444c0e6bbb3bf298b2ff2d69315a
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/msg/GyroMeasurements.msg
@@ -0,0 +1,4 @@
+string vehicleName
+float64 rollrate
+float64 pitchrate
+float64 yawrate
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/Setpoint.msg b/dfall_ws/src/dfall_pkg/msg/Setpoint.msg
index b3f94d9fbf6f8c0893c93e6e50e437a52bc71b6e..eb02bd5d08d8507d3de2b3df7ea3f7a61f13a7ea 100644
--- a/dfall_ws/src/dfall_pkg/msg/Setpoint.msg
+++ b/dfall_ws/src/dfall_pkg/msg/Setpoint.msg
@@ -1,4 +1,4 @@
float64 x
float64 y
float64 z
-float64 yaw
+float64 yaw
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/UnlabeledMarker.msg b/dfall_ws/src/dfall_pkg/msg/UnlabeledMarker.msg
index c05b0c57394b6b2aa42c0e47fb51a3d9a6e32a83..a0b6cea0a09e4f9c1d26416146dce2602718aec2 100644
--- a/dfall_ws/src/dfall_pkg/msg/UnlabeledMarker.msg
+++ b/dfall_ws/src/dfall_pkg/msg/UnlabeledMarker.msg
@@ -1,4 +1,4 @@
uint32 index
float64 x
float64 y
-float64 z
+float64 z
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/msg/ViconData.msg b/dfall_ws/src/dfall_pkg/msg/ViconData.msg
index c263cdf9116fa84be514a184e4cd6b422308cb7c..ae8186553d2d9b9ef539d31c122e8b4b9d0fb4ff 100755
--- a/dfall_ws/src/dfall_pkg/msg/ViconData.msg
+++ b/dfall_ws/src/dfall_pkg/msg/ViconData.msg
@@ -1,3 +1,2 @@
-CrazyflieData[] crazyflies
-UnlabeledMarker[] markers
-
+FlyingVehicleState[] crazyflies
+UnlabeledMarker[] markers
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
new file mode 100755
index 0000000000000000000000000000000000000000..58a5f31dc2a15f3bc00ced58fa91bf57ce4f8047
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
@@ -0,0 +1,29 @@
+# Frequency of requesting the onboard state estimate, in [milliseconds]
+cfStateEstimate_polling_period: 20
+
+# Flag for whether to use height as a trigger for
+# publishing a motors-OFF command
+isEnabled_strictSafety: true
+maxHeight_for_strictSafety_meters: 1.8
+
+
+# ----------------------------------------------- #
+# THE FOLLOWING ARE USED FOR EMULATING A CRAZYFLIE:
+
+# SIMULATION FREQUENCY FOR EMULATING A CRAZYFLIE
+cfSimulation_frequency: 200
+
+# HOW OFTEN TO PUBLISH THE SIMULATION STATE AS AN
+# ONBOARD ESTIMATE
+# > i.e., this integer divided by the
+# "cfSimulation_frequency" gives the simulation
+# equivalent of "cfStateEstimate_polling_period"
+cfSimulation_stateEstimate_sendEvery: 4
+
+# 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/param/Crazyflie.db b/dfall_ws/src/dfall_pkg/param/Crazyflie.db
new file mode 100644
index 0000000000000000000000000000000000000000..90468eef837ca4d76cedd00054457304ade33888
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/param/Crazyflie.db
@@ -0,0 +1 @@
+1,CF01,0/0/2M/E7E7E7E701,0,-0.8,-0.8,-0.2,0.8,0.8,1.8
diff --git a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
index 2ae871658da4c35efa4ee54a1ac47f9f2245e164..a880a37cb6fa6a1e720be4a788377521c55f78b0 100644
--- a/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/DefaultController.yaml
@@ -69,7 +69,7 @@ landing_spin_motors_time: 1.5
mass : 30
# Frequency of the controller, in [hertz]
-control_frequency : 200
+control_frequency : 50
# Quadratic motor regression equation (a0, a1, a2)
motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
@@ -122,6 +122,9 @@ integratorGain_forTauYaw : 0.05
# each of (x,y,z,roll,pitch,yaw)
estimator_method : 1
+# Frequency of the estimator, in [hertz]
+estimator_frequency : 100
+
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
diff --git a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
index 6edb428394692da236ffb24a4c522657fb19c334..94b423c16324d79819f98455a74817a37970fa08 100755
--- a/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/FlyingAgentClientConfig.yaml
@@ -14,17 +14,22 @@ testMotorsController: "TestMotorsControllerService/TestMotorsController"
# Controller for requests manoeuvres to be performed
defaultController_requestManoeuvre: "DefaultControllerService/RequestManoeuvre"
+# Flags for which measurement streams to use
+shouldUse_mocapMeasurements: false
+shouldUse_onboardEstimate: true
+
# Flag for whether to use angle for switching
# to the Default Controller
isEnabled_strictSafety: true
maxTiltAngle_for_strictSafety_degrees: 70
-# Number of consecutive occulsions that will deem the
-# object as "long-term" occuled
-consecutive_occulsions_threshold: 30
+# Number of consecutive invalid measurements that
+# will deem the object as "long-term" unavailable
+consecutive_invalid_threshold: 30
-# Time out duration after which Mocap is considered unavailable
-mocap_timeout_duration: 2.0
+# Time out duration after which measurements are
+# considered unavailable
+measurement_timeout_duration: 2.0
# Flag for whether the take-off should be performed
# with the default controller
diff --git a/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml b/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml
index 898441b35b38134c07a687bf87ba38c3bd6b3251..633cacb786103403cd3949ed6ea6df1fcd88764d 100644
--- a/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/MocapEmulatorConfig.yaml
@@ -1,5 +1,5 @@
# FREQUENCY OF THE MOTION CAPTURE EMULATOR [Hertz]
-mocap_frequency : 200.0
+mocap_frequency : 100.0
# QUADRATIC MOTOR REGRESSION EQUATION (a0, a1, a2)
# > [Newtons] = a2 [cmd]^2 + a1 [cmd] + a0
diff --git a/dfall_ws/src/dfall_pkg/param/StudentController.yaml b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
index 737fea73b48980c4701f4c8f15ec4a9f74d3e076..8a867e07c5b6a454d926ffb792ae4692a22b19e7 100644
--- a/dfall_ws/src/dfall_pkg/param/StudentController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/StudentController.yaml
@@ -4,9 +4,11 @@ mass : 28
# Frequency of the controller, in hertz
control_frequency : 200
-# Quadratic motor regression equation (a0, a1, a2)
-motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
-
# The default setpoint, the ordering is (x,y,z,yaw),
# with unit [meters,meters,meters,radians]
-default_setpoint : [0.0, 0.0, 0.4, 0.0]
\ No newline at end of file
+default_setpoint : [0.0, 0.0, 0.4, 0.0]
+
+# PID controller gain matrices
+proportionalgain_for_z : 4
+derivativegain_for_z : 1
+integralgain_for_z : 0.5
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
index b61a3e84ba4f77f9e3e914e2d3741baeb4212d21..94ce721da5000f068ef9ec3f5581dc7565d3f456 100644
--- a/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
+++ b/dfall_ws/src/dfall_pkg/param/YamlFileNames.yaml
@@ -1,4 +1,11 @@
-dictionary : {
- 'FlyingAgentClientConfig' : 'FlyingAgentClientConfig' ,
- 'SafeController' : 'SafeController'
-}
+filenames : [
+ 'BatteryMonitor',
+ 'FlyingAgentClientConfig',
+ 'CsoneControllerService',
+ 'DefaultControllerService',
+ 'PickerControllerService',
+ 'RemoteControllerService',
+ 'StudentControllerService',
+ 'TemplateControllerService',
+ 'TuningControllerService'
+]
diff --git a/dfall_ws/src/dfall_pkg/src/CrazyflieIO.cpp b/dfall_ws/src/dfall_pkg/src/classes/CrazyflieIO.cpp
similarity index 99%
rename from dfall_ws/src/dfall_pkg/src/CrazyflieIO.cpp
rename to dfall_ws/src/dfall_pkg/src/classes/CrazyflieIO.cpp
index 0f8625211fa2df7d2106313f89595d34c04a6c4b..a66320ce8aaed845ce9b1f9012aa15d845c40bd9 100644
--- a/dfall_ws/src/dfall_pkg/src/CrazyflieIO.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/CrazyflieIO.cpp
@@ -36,7 +36,7 @@
// ----------------------------------------------------------------------------------
-#include "CrazyflieIO.h"
+#include "classes/CrazyflieIO.h"
#include <stdlib.h>
#include <ros/ros.h>
#include <ros/package.h>
diff --git a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
index c0865950e3f37775a4998a69faf505e1a7942634..63e50c816c1214ba7efb80389d09ac8c6a11845d 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/GetParamtersAndNamespaces.cpp
@@ -78,79 +78,105 @@ std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
{
std::string val;
if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
-}
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
- float val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
- int val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
{
- bool val;
- if(!nodeHandle.getParam(name, val))
- {
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val;
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
}
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+int getParameterStringVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+
+int getParameterStringVectorUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<std::string>& val)
{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
}
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
}
+int getParameterFloatVectorUnnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+
+
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
{
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- return val.size();
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
}
@@ -242,4 +268,47 @@ bool checkMessageHeader( int agentID , bool shouldCheckForAgentID , const std::v
// If the function made it to here, then the message is
// NOT relevant, hence return false
return false;
+}
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+// FUNCTION FOR CONVERTING NEWTON TO MOTOR COMMAND
+
+float newtons2cmd_for_crazyflie( float thrust)
+{
+ // Compute the 16-bit command that would produce the requested
+ // "thrust" based on the quadratic mapping that is described
+ // by the coefficients in the "yaml_motorPoly" variable.
+ static float motorPoly[3] = {5.484560e-4, 1.032633e-6, 2.130295e-11};
+
+ float cmd_16bit = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Clip the cmd_16bit to avoid wrapping
+ if (cmd_16bit > 60000)
+ {
+ cmd_16bit = 60000;
+ }
+ else
+ {
+ if (cmd_16bit < 2000)
+ {
+ cmd_16bit = 0;
+ }
+ }
+
+ // Return the result
+ return cmd_16bit;
}
\ 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
index ff922c8fb7b3048a4ff5003280478d36cef5cf2a..b36f14648625c46374f8e9ee68fe48287412f18f 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
@@ -40,11 +40,27 @@
+// EMPTY CONSTRUCTOR
+QuadrotorSimulator::QuadrotorSimulator () :
+ m_gen_thrust( (std::random_device())() ),
+ m_dist_thrust(-0.03,0.03),
+ 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 default ID string
+ this->set_id_string( "CF00" );
+ // Set the default mass
+ this->set_mass_in_kg( 0.032 );
+}
-
+// DEFAULT CONSTRUCTOR
QuadrotorSimulator::QuadrotorSimulator ( std::string id ) :
m_gen_thrust( (std::random_device())() ),
- m_dist_thrust(-0.02,0.02),
+ m_dist_thrust(-0.03,0.03),
m_gen_body_x( (std::random_device())() ),
m_dist_body_x(-0.1,0.1),
m_gen_body_y( (std::random_device())() ),
@@ -53,11 +69,14 @@ QuadrotorSimulator::QuadrotorSimulator ( std::string id ) :
m_dist_body_z(-0.1,0.1)
{
// Set the input argument to the ID string
- this->m_id_string = id;
+ this->set_id_string( id );
+ //this->m_id_string = id;
// Set the default mass
- this->m_mass_in_kg = 0.032;
+ this->set_mass_in_kg( 0.032 );
+ //this->m_mass_in_kg = mass;
}
+// PARAMETERIZED CONSTRUCTOR
QuadrotorSimulator::QuadrotorSimulator ( std::string id , float mass ) :
m_gen_thrust( (std::random_device())() ),
m_dist_thrust(-0.1*mass*GRAVITY,0.1*mass*GRAVITY),
@@ -69,9 +88,9 @@ QuadrotorSimulator::QuadrotorSimulator ( std::string id , float mass ) :
m_dist_body_z(-0.1,0.1)
{
// Set the input argument to the ID string
- this->m_id_string = id;
+ this->set_id_string( id );
// Set the input argument to the mass
- this->m_mass_in_kg = mass;
+ this->set_mass_in_kg( mass );
}
@@ -92,6 +111,36 @@ QuadrotorSimulator::QuadrotorSimulator ( std::string id , float mass ) :
// ----------------------------------------------------------------------------------
+// Function to get the ID as a string
+std::string QuadrotorSimulator::get_id_string()
+{
+ return this->m_id_string;
+}
+
+// Function to set the ID as a string
+void QuadrotorSimulator::set_id_string(std::string new_id_string)
+{
+ this->m_id_string = new_id_string;
+}
+
+// Function to get the mass in kilograms
+float QuadrotorSimulator::get_mass_in_kg()
+{
+ return this->m_mass_in_kg;
+}
+
+// Function to set the mass in kilograms
+void QuadrotorSimulator::set_mass_in_kg(float new_mass_in_kg)
+{
+ this->m_mass_in_kg = new_mass_in_kg;
+}
+
+// Function to get the flying state
+bool QuadrotorSimulator::get_isFlying()
+{
+ return this->m_isFlying;
+}
+
// Function to simulate the quadrotor for one time step
void QuadrotorSimulator::simulate_for_one_time_step(float deltaT)
{
@@ -201,6 +250,10 @@ void QuadrotorSimulator::simulate_for_one_time_step(float deltaT)
this->m_euler_angles[0] = new_roll;
this->m_euler_angles[1] = new_pitch;
this->m_euler_angles[2] = new_yaw;
+ // > For the Euler angular velocities
+ this->m_euler_velocities[0] = command_body_rate_x;
+ this->m_euler_velocities[1] = command_body_rate_y;
+ this->m_euler_velocities[2] = command_body_rate_z;
@@ -210,6 +263,8 @@ void QuadrotorSimulator::simulate_for_one_time_step(float deltaT)
{
// Reset the state
this->reset();
+ // Inform the user
+ ROS_INFO_STREAM("[QUADROTOR SIMULATOR] Quadrotor \"" << this->m_id_string << "\" reset due to roll or pitch exceeding 80 degrees." );
}
} // END OF: "if (this->m_isFlying)"
@@ -274,12 +329,6 @@ void QuadrotorSimulator::update_commanding_agent_id( int 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)
{
@@ -322,33 +371,44 @@ void QuadrotorSimulator::print_details()
void QuadrotorSimulator::control_commands_callback( const dfall_pkg::ControlCommand & msg )
{
// Get the type of the command
- int command_type = msg.onboardControllerType;
+ int x_mode = msg.xControllerMode;
+ int y_mode = msg.yControllerMode;
+ int z_mode = msg.zControllerMode;
+ int yaw_mode = msg.yawControllerMode;
+
+ int command_type = 0;
+ if (
+ (x_mode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE)
+ && (y_mode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE)
+ && (z_mode==CF_ONBOARD_CONTROLLER_MODE_OFF)
+ && (yaw_mode==CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE)
+ )
+ {
+ command_type = 2;
+ }
+ if (
+ (x_mode==CF_ONBOARD_CONTROLLER_MODE_OFF)
+ && (y_mode==CF_ONBOARD_CONTROLLER_MODE_OFF)
+ && (z_mode==CF_ONBOARD_CONTROLLER_MODE_OFF)
+ && (yaw_mode==CF_ONBOARD_CONTROLLER_MODE_OFF)
+ )
+ {
+ command_type = 1;
+ }
+
// Switch based on the command type
// Adapt to the new state
switch(command_type)
{
- case CF_COMMAND_TYPE_MOTORS:
+ case 1:
{
// 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:
+
+ case 2:
{
// Put the command into the class variables
this->update_current_control_command_rate(msg);
@@ -360,6 +420,20 @@ void QuadrotorSimulator::control_commands_callback( const dfall_pkg::ControlComm
//}
break;
}
+
+ default:
+ {
+ 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 NOT of type MOTORS or ANGULAR_RATE");
+ }
+ break;
+ }
}
}
@@ -377,9 +451,9 @@ void QuadrotorSimulator::update_current_control_command_rate( const dfall_pkg::C
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;
+ this->m_current_command_body_rate_x = msg.yControllerSetpoint;
+ this->m_current_command_body_rate_y = msg.xControllerSetpoint;
+ this->m_current_command_body_rate_z = msg.yawControllerSetpoint;
}
// Callback for receiving flying state updates
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/AgentStatusForWebInterface.cpp b/dfall_ws/src/dfall_pkg/src/nodes/AgentStatusForWebInterface.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..612fd244f8fed4808e77624b15091a7e1964784b
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/AgentStatusForWebInterface.cpp
@@ -0,0 +1,521 @@
+// Copyright (C) 2020, The University of Melbourne, Department of Electrical and Electronic Engineering (EEE), 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 provides data to the web interface
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/AgentStatusForWebInterface.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
+// ----------------------------------------------------------------------------------
+
+
+
+// CALLBACKS FOR MESSAGES WITH THE STATUS OF THINGS
+
+// > For the status of the crazyradio
+void crazyRadioStatusCallback(const std_msgs::Int32& msg)
+{
+ m_crazyradio_status = msg.data;
+}
+
+// > For the battery level
+void newBatteryLevelCallback(const std_msgs::Int32& msg)
+{
+ m_battery_level = msg.data;
+}
+
+// > For the flying state of the agent
+void agentOperatingStateCallback(const std_msgs::Int32& msg)
+{
+ m_agent_operating_state = msg.data;
+}
+
+// > For the instant controller
+void instantControllerChangedCallback(const std_msgs::Int32& msg)
+{
+ m_instant_controller = msg.data;
+}
+
+// > For the Default Controller Setpoint
+void defaultControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint)
+{
+ m_setpoint_default[0] = newSetpoint.x;
+ m_setpoint_default[1] = newSetpoint.y;
+ m_setpoint_default[2] = newSetpoint.z;
+ m_setpoint_default[3] = newSetpoint.yaw;
+}
+
+// > For the Student Controller Setpoint
+void studentControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint)
+{
+ m_setpoint_student[0] = newSetpoint.x;
+ m_setpoint_student[1] = newSetpoint.y;
+ m_setpoint_student[2] = newSetpoint.z;
+ m_setpoint_student[3] = newSetpoint.yaw;
+}
+
+// > For the Student Controller Debug Values
+void studentControllerDebugValuesCallback(const DebugMsg& newDebugMsg)
+{
+ m_debug_values_student[0] = newDebugMsg.value_1;
+ m_debug_values_student[1] = newDebugMsg.value_2;
+ m_debug_values_student[2] = newDebugMsg.value_3;
+ m_debug_values_student[3] = newDebugMsg.value_4;
+ m_debug_values_student[4] = newDebugMsg.value_5;
+ m_debug_values_student[5] = newDebugMsg.value_6;
+ m_debug_values_student[6] = newDebugMsg.value_7;
+ m_debug_values_student[7] = newDebugMsg.value_8;
+ m_debug_values_student[8] = newDebugMsg.value_9;
+ m_debug_values_student[9] = newDebugMsg.value_10;
+}
+
+// > For the State Estimate from the Crazyflie
+void cfStateEstimateCallback(const FlyingVehicleState & newStateEstimate)
+{
+ // > For (x,y,z) positions
+ m_cf_state_estimate_xyz_rpy[0] = newStateEstimate.x;
+ m_cf_state_estimate_xyz_rpy[1] = newStateEstimate.y;
+ m_cf_state_estimate_xyz_rpy[2] = newStateEstimate.z;
+ // > For (roll,pitch,yaw) orientation
+ m_cf_state_estimate_xyz_rpy[3] = newStateEstimate.roll;
+ m_cf_state_estimate_xyz_rpy[4] = newStateEstimate.pitch;
+ m_cf_state_estimate_xyz_rpy[5] = newStateEstimate.yaw;
+}
+
+
+
+
+
+// SERVICE CALLBACK FOR PROVIDING STATUS TO THE WEB INTERFACE
+bool statusForWebInterfaceCallback(IntStringService::Request &request, IntStringService::Response &response)
+{
+ // Get the CrazyRadio status as a string
+ std::string crazyradio_status_string;
+ switch (m_crazyradio_status)
+ {
+ case CRAZY_RADIO_STATE_CONNECTED:
+ {
+ crazyradio_status_string = "connected";
+ break;
+ }
+ case CRAZY_RADIO_STATE_CONNECTING:
+ {
+ crazyradio_status_string = "connecting";
+ break;
+ }
+ case CRAZY_RADIO_STATE_DISCONNECTED:
+ {
+ crazyradio_status_string = "disconnected";
+ break;
+ }
+ default:
+ {
+ crazyradio_status_string = "unknown";
+ break;
+ }
+ }
+
+
+ // Get the Battery level as a string
+ std::string battery_level_string;
+ switch (m_battery_level)
+ {
+ case BATTERY_LEVEL_000:
+ {
+ battery_level_string = "000";
+ break;
+ }
+ case BATTERY_LEVEL_010:
+ {
+ battery_level_string = "010";
+ break;
+ }
+ case BATTERY_LEVEL_020:
+ {
+ battery_level_string = "020";
+ break;
+ }
+ case BATTERY_LEVEL_030:
+ {
+ battery_level_string = "030";
+ break;
+ }
+ case BATTERY_LEVEL_040:
+ {
+ battery_level_string = "040";
+ break;
+ }
+ case BATTERY_LEVEL_050:
+ {
+ battery_level_string = "050";
+ break;
+ }
+ case BATTERY_LEVEL_060:
+ {
+ battery_level_string = "060";
+ break;
+ }
+ case BATTERY_LEVEL_070:
+ {
+ battery_level_string = "070";
+ break;
+ }
+ case BATTERY_LEVEL_080:
+ {
+ battery_level_string = "080";
+ break;
+ }
+ case BATTERY_LEVEL_090:
+ {
+ battery_level_string = "090";
+ break;
+ }
+ case BATTERY_LEVEL_100:
+ {
+ battery_level_string = "100";
+ break;
+ }
+ case BATTERY_LEVEL_UNAVAILABLE:
+ {
+ battery_level_string = "unavailable";
+ break;
+ }
+ default:
+ {
+ battery_level_string = "unavailable";
+ break;
+ }
+ }
+
+ // Get the Flying status as a string
+ std::string flying_state_string;
+ switch (m_agent_operating_state)
+ {
+ case STATE_MOTORS_OFF:
+ {
+ flying_state_string = "motorsoff";
+ break;
+ }
+ case STATE_TAKE_OFF:
+ {
+ flying_state_string = "takeoff";
+ break;
+ }
+ case STATE_FLYING:
+ {
+ flying_state_string = "flying";
+ break;
+ }
+ case STATE_LAND:
+ {
+ flying_state_string = "land";
+ break;
+ }
+ case STATE_UNAVAILABLE:
+ {
+ flying_state_string = "unavailable";
+ break;
+ }
+ default:
+ {
+ flying_state_string = "unavailable";
+ break;
+ }
+ }
+
+ // Get the Flying status as a string
+ std::string instant_controller_string;
+ switch (m_instant_controller)
+ {
+ case DEFAULT_CONTROLLER:
+ {
+ instant_controller_string = "default";
+ break;
+ }
+ case DEMO_CONTROLLER:
+ {
+ instant_controller_string = "demo";
+ break;
+ }
+ case STUDENT_CONTROLLER:
+ {
+ instant_controller_string = "student";
+ break;
+ }
+ case MPC_CONTROLLER:
+ {
+ instant_controller_string = "mpc";
+ break;
+ }
+ case REMOTE_CONTROLLER:
+ {
+ instant_controller_string = "remote";
+ break;
+ }
+ case TUNING_CONTROLLER:
+ {
+ instant_controller_string = "tuning";
+ break;
+ }
+ case PICKER_CONTROLLER:
+ {
+ instant_controller_string = "picker";
+ break;
+ }
+ case TEMPLATE_CONTROLLER:
+ {
+ instant_controller_string = "template";
+ break;
+ }
+ case CSONE_CONTROLLER:
+ {
+ instant_controller_string = "csone";
+ break;
+ }
+ case TESTMOTORS_CONTROLLER:
+ {
+ instant_controller_string = "testmotors";
+ break;
+ }
+ default:
+ {
+ instant_controller_string = "none";
+ break;
+ }
+ }
+
+ // Concatenate the json together using a string stream
+ std::stringstream ss;
+ ss << "{";
+ ss << "\u0022crazyradiostatus\u0022: \u0022" << crazyradio_status_string << "\u0022";
+ ss << " , ";
+ ss << "\u0022batterylevel\u0022: \u0022" << battery_level_string << "\u0022";
+ ss << " , ";
+ ss << "\u0022flyingstate\u0022: \u0022" << flying_state_string << "\u0022";
+ ss << " , ";
+ ss << "\u0022instantcontroller\u0022: \u0022" << instant_controller_string << "\u0022";
+ ss << " , ";
+ ss << "\u0022setpointdefault\u0022:";
+ ss << "{";
+ ss << "\u0022x\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_default[0];
+ ss << " , ";
+ ss << "\u0022y\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_default[1];
+ ss << " , ";
+ ss << "\u0022z\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_default[2];
+ ss << " , ";
+ ss << "\u0022yaw\u0022: " << std::setprecision(1) << std::fixed << m_setpoint_default[3]*RAD2DEG;
+ ss << "}";
+ ss << " , ";
+ ss << "\u0022setpointstudent\u0022:";
+ ss << "{";
+ ss << "\u0022x\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_student[0];
+ ss << " , ";
+ ss << "\u0022y\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_student[1];
+ ss << " , ";
+ ss << "\u0022z\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_student[2];
+ ss << " , ";
+ ss << "\u0022yaw\u0022: " << std::setprecision(1) << std::fixed << m_setpoint_student[3]*RAD2DEG;
+ ss << "}";
+ ss << " , ";
+ ss << "\u0022stateestimate\u0022:";
+ ss << "{";
+ ss << "\u0022x\u0022: " << std::setprecision(3) << std::fixed << m_cf_state_estimate_xyz_rpy[0];
+ ss << " , ";
+ ss << "\u0022y\u0022: " << std::setprecision(3) << std::fixed << m_cf_state_estimate_xyz_rpy[1];
+ ss << " , ";
+ ss << "\u0022z\u0022: " << std::setprecision(3) << std::fixed << m_cf_state_estimate_xyz_rpy[2];
+ ss << " , ";
+ ss << "\u0022roll\u0022: " << std::setprecision(1) << std::fixed << m_cf_state_estimate_xyz_rpy[3]*RAD2DEG;
+ ss << " , ";
+ ss << "\u0022pitch\u0022: " << std::setprecision(1) << std::fixed << m_cf_state_estimate_xyz_rpy[4]*RAD2DEG;
+ ss << " , ";
+ ss << "\u0022yaw\u0022: " << std::setprecision(1) << std::fixed << m_cf_state_estimate_xyz_rpy[5]*RAD2DEG;
+ ss << "}";
+ ss << " , ";
+ ss << "\u0022debugvaluesstudent\u0022:";
+ ss << "{";
+ ss << "\u0022value1\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[0];
+ ss << " , ";
+ ss << "\u0022value2\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[1];
+ ss << " , ";
+ ss << "\u0022value3\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[2];
+ ss << " , ";
+ ss << "\u0022value4\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[3];
+ ss << " , ";
+ ss << "\u0022value5\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[4];
+ ss << " , ";
+ ss << "\u0022value6\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[5];
+ ss << " , ";
+ ss << "\u0022value7\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[6];
+ ss << " , ";
+ ss << "\u0022value8\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[7];
+ ss << " , ";
+ ss << "\u0022value9\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[8];
+ ss << " , ";
+ ss << "\u0022value10\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[9];
+ ss << "}";
+ ss << "}";
+
+ //std::string s = ss.str();
+ // Put the string into the response
+ //response.data = "test of service for web interface";
+ //response.data = "{\"crazyradiostatus\": \"%d\"}", m_crazyradio_status;
+ response.data = ss.str();
+
+ // Return success
+ return true;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// 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, "AgentStatusForWebInterface");
+
+ // 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 "AgentStatusForWebInterface" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[AGENT STATUS FOR WEB INTERFACE] ros::this_node::getNamespace() = " << m_namespace);
+
+
+
+
+
+ // SUBSCRIBERS
+
+ // FOR THE CRAZY RADIO STATUS:
+ // > Get a node handle to the Crazy Radio
+ std::string namespace_to_crazyradio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_crazyradio(namespace_to_crazyradio);
+ // > Subscribe to the topic
+ ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazyradio.subscribe("CrazyRadioStatus", 1, crazyRadioStatusCallback);
+
+ // FOR THE BATTERY LEVEL
+ // > Get a node handle to the Battery Monitor
+ std::string namespace_to_BatteryMonitor = m_namespace + "/BatteryMonitor";
+ ros::NodeHandle nodeHandle_to_BatteryMonitor(namespace_to_BatteryMonitor);
+ // > Subscribe to the topic
+ ros::Subscriber newBatteryLevelSubscriber = nodeHandle_to_BatteryMonitor.subscribe("Level", 1, newBatteryLevelCallback);
+
+ // FOR THE FLYING STATE OF THE AGENT
+ // > Get a node handle to the Flying Agent Client
+ std::string namespace_to_FlyingAgentClient = m_namespace + "/FlyingAgentClient";
+ ros::NodeHandle nodeHandle_to_FlyingAgentClient(namespace_to_FlyingAgentClient);
+ // > Subscribe to the topic
+ ros::Subscriber agentOperatingStateSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("FlyingState", 1, agentOperatingStateCallback);
+
+ // FOR THE INSTANT CONTROLLER
+ // > Subscribe to the topic
+ ros::Subscriber instantControllerSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("ControllerUsed", 1, instantControllerChangedCallback);
+
+ // FOR THE DEFAULT CONTROLLER SETPOINT
+ // > Get a node handle to the Default Controller Service
+ std::string namespace_to_DefaultController = m_namespace + "/DefaultControllerService";
+ ros::NodeHandle nodeHandle_to_DefaultController(namespace_to_DefaultController);
+ // > Subscribe to the topic
+ ros::Subscriber defaultControllerSetpointChangedSubscriber = nodeHandle_to_DefaultController.subscribe("SetpointChanged", 1, defaultControllerSetpointChangedCallback);
+
+ // FOR THE STUDENT CONTROLLER SETPOINT
+ // > Get a node handle to the Student Controller Service
+ std::string namespace_to_StudentController = m_namespace + "/StudentControllerService";
+ ros::NodeHandle nodeHandle_to_StudentController(namespace_to_StudentController);
+ // > Subscribe to the topic
+ ros::Subscriber studentControllerSetpointChangedSubscriber = nodeHandle_to_StudentController.subscribe("SetpointChanged", 1, studentControllerSetpointChangedCallback);
+
+ // FOR THE DEBUG VALUES OF THE STUDENT CONTROLLER
+ // > Subscribe to the topic
+ ros::Subscriber studentControllerDebugValuesSubscriber = nodeHandle_to_StudentController.subscribe("DebugTopic", 1, studentControllerDebugValuesCallback);
+
+ // FOR THE STATE ESTIMATE FROM THE CRAZYFLIE
+ // > Get a node handle to the Crazy Radio node
+ std::string namespace_to_CrazyRadio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_CrazyRadio(namespace_to_CrazyRadio);
+ // > Subscribe to the topic
+ ros::Subscriber cfStateEstimateSubscriber = nodeHandle_to_CrazyRadio.subscribe("CFStateEstimate", 1, cfStateEstimateCallback);
+
+
+
+
+
+ // SERVICE SERVERS
+
+ // Service callback for provide status to the Web Interface
+ ros::ServiceServer statusForWebInterfaceService = nodeHandle.advertiseService("StatusAsJson", statusForWebInterfaceCallback);
+
+
+
+
+
+ // Inform the user the this node is ready
+ ROS_INFO("[AGENT STATUS FOR WEB INTERFACE] Ready :-)");
+ // Spin as a single-thread node
+ ros::spin();
+
+ return 0;
+}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
index 93f5a305abf457a3f457884c29998b656dedb357..748c5c374d5794b5c31974886337662eb49ca329 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
@@ -488,7 +488,11 @@ int main(int argc, char* argv[])
// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
- // Call the class function that loads the parameters for this class.
+ // Call the class function that loads the parameters
+ // from the "BatteryMonitor.yaml" file.
+ // > This is possible because that YAML file is added
+ // to the parameter service when launched via the
+ // "agent.launch" file.
fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
index f056425244ab54703064bec427a62bf31c07f615..6ce70b498bafb2d42bcd365c82b022f26ea350c2 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
@@ -142,6 +142,9 @@ void connected_callback(const ros::TimerEvent&)
// Start the battery voltage updates
m_timer_battery_voltage_updates.start();
+
+ // Start the state estimate updates
+ m_timer_state_estimate_update.start();
}
void connection_failed()
@@ -152,6 +155,8 @@ void connection_failed()
change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
// Stop the battery voltage updates
m_timer_battery_voltage_updates.stop();
+ // Stop the state estimate updates
+ m_timer_state_estimate_update.stop();
}
void connection_lost()
@@ -162,6 +167,8 @@ void connection_lost()
change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
// Stop the battery voltage updates
m_timer_battery_voltage_updates.stop();
+ // Stop the state estimate updates
+ m_timer_state_estimate_update.stop();
}
@@ -170,9 +177,10 @@ void disconnected_callback(const ros::TimerEvent&)
{
// Update status to DISCONNECTED
change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
-
// Stop the battery voltage updates
m_timer_battery_voltage_updates.stop();
+ // Stop the state estimate updates
+ m_timer_state_estimate_update.stop();
}
@@ -261,7 +269,7 @@ bool getCurrentCrazyRadioStatusServiceCallback(IntIntService::Request &request,
}
-void update_battery_voltage_callback(const ros::TimerEvent&)
+void timerCallback_update_battery_voltage(const ros::TimerEvent&)
{
// Declare static paramters for the voltage
static float voltage_current = 4.2;
@@ -286,6 +294,84 @@ void update_battery_voltage_callback(const ros::TimerEvent&)
+void timerCallback_update_cfStateEstimate(const ros::TimerEvent&)
+{
+ // Declare static counter for publshing the state estimates
+ static int counter_since_last_send = 3;
+
+ // Increment the counter
+ counter_since_last_send++;
+
+ // Simulate the quadrotor for one time step
+ m_quadrotor_sim.simulate_for_one_time_step( yaml_cfSimulation_deltaT_in_seconds );
+
+ // Perform safety check if required
+ if (yaml_isEnabled_strictSafety)
+ {
+ // Estimate the height at the next measurement
+ float height_at_next_measurement = m_quadrotor_sim.m_position[2] + yaml_cfSimulation_deltaT_in_seconds * m_quadrotor_sim.m_velocity[2];
+ // Turn-off if too high
+ if (height_at_next_measurement > yaml_maxHeight_for_strictSafety_meters)
+ {
+ // Send the MOTORS-OFF command to the Flying Agent Client
+ IntWithHeader msg;
+ msg.shouldCheckForAgentID = false;
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ flyingAgentClientCommandPublisher.publish(msg);
+ // Inform the user
+ ROS_ERROR_STREAM("[CRAZY RADIO] Height safety check failed, measured = " << height_at_next_measurement << ", max allowed = " << yaml_maxHeight_for_strictSafety_meters );
+ }
+ }
+
+ // Local variable for the data of this quadrotor
+ FlyingVehicleState quadrotor_data;
+
+ // Fill in the details
+ // > For the type
+ quadrotor_data.type = FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE;
+ // > For the name
+ quadrotor_data.vehicleName = m_quadrotor_sim.get_id_string();
+ // > For the occulsion flag
+ quadrotor_data.isValid = true;
+ // > For position
+ quadrotor_data.x = m_quadrotor_sim.m_position[0];
+ quadrotor_data.y = m_quadrotor_sim.m_position[1];
+ quadrotor_data.z = m_quadrotor_sim.m_position[2];
+ // > For velocities
+ quadrotor_data.vx = m_quadrotor_sim.m_velocity[0];
+ quadrotor_data.vy = m_quadrotor_sim.m_velocity[1];
+ quadrotor_data.vz = m_quadrotor_sim.m_velocity[2];
+ // > For euler angles
+ quadrotor_data.roll = m_quadrotor_sim.m_euler_angles[0];
+ quadrotor_data.pitch = m_quadrotor_sim.m_euler_angles[1];
+ quadrotor_data.yaw = m_quadrotor_sim.m_euler_angles[2];
+ // > For euler angular rates
+ quadrotor_data.rollRate = m_quadrotor_sim.m_euler_velocities[0];
+ quadrotor_data.pitchRate = m_quadrotor_sim.m_euler_velocities[1];
+ quadrotor_data.yawRate = m_quadrotor_sim.m_euler_velocities[2];
+ // > For the acquiring time
+ quadrotor_data.acquiringTime = 0.0;
+
+ // Publish the current state on the "CFSimulationState"
+ // topic that is used by the MocapEmulator node
+ cfSimulationStatePublisher.publish(quadrotor_data);
+
+
+ // Publish the current state on the "CFStateEstimate"
+ // topic that emulates the Crazyflie
+ // > If counter indicates to do so
+ if (counter_since_last_send >= yaml_cfSimulation_stateEstimate_sendEvery)
+ {
+ cfStateEstimatorPublisher.publish(quadrotor_data);
+ // Reset the counter
+ counter_since_last_send = 0;
+ }
+}
+
+
+
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
// L O O A A D D
@@ -390,6 +476,52 @@ void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle)
+
+void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the file:
+ // BatteryMonitor.yaml
+
+ // Add the "CrazyRadioConfig" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CrazyRadioConfig");
+
+ // FLAG FOR WHETHER TO USE HEIGHT AS A TRIGGER FOR
+ // PUBLISHING A MOTORS-OFF COMMAND
+ yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
+ yaml_maxHeight_for_strictSafety_meters = getParameterFloat(nodeHandle_for_paramaters,"maxHeight_for_strictSafety_meters");
+
+ // SIMULATION FREQUENCY FOR EMULATING A CRAZYFLIE [Hertz]
+ float yaml_cfSimulation_frequency = getParameterFloat(nodeHandle_for_paramaters,"cfSimulation_frequency");
+
+ // HOW OFTEN TO PUBLISH THE SIMULATION STATE AS AN ONBOARD ESTIMATE
+ int yaml_cfSimulation_stateEstimate_sendEvery = getParameterInt(nodeHandle_for_paramaters,"cfSimulation_stateEstimate_sendEvery");
+
+ // THE COEFFICIENT OF THE 16-BIT COMMAND TO THRUST CONVERSION
+ getParameterFloatVectorKnownLength(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("[CRAZY RADIO EMULATOR] DEBUGGING: the fetched CrazyRadioConfig/cfSimulation_stateEstimate_sendEvery = " << yaml_cfSimulation_stateEstimate_sendEvery);
+
+
+
+ // PROCESS THE PARAMTERS
+ // Convert from Hertz to second
+ yaml_cfSimulation_deltaT_in_seconds = 1.0 / yaml_cfSimulation_frequency;
+
+ // DEBUGGING: Print out one of the processed values
+ ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] DEBUGGING: after processing yaml_cfSimulation_deltaT_in_seconds = " << yaml_cfSimulation_deltaT_in_seconds);
+}
+
+
+
+
+
// ----------------------------------------------------------------------------------
// M M A III N N
// MM MM A A I NN N
@@ -489,13 +621,49 @@ int main(int argc, char* argv[])
// Call the class function that loads the parameters for this class.
fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ // Call the class function that loads the parameters for this class.
+ fetchCrazyRadioConfigYamlParameters(nodeHandle);
- // TIMER FOR THE BATTERY VOLTAGE
- m_timer_battery_voltage_updates = nodeHandle.createTimer(ros::Duration(yaml_battery_polling_period_in_seconds), update_battery_voltage_callback, false);
+
+ // INITIALISE TIMER FOR THE BATTERY VOLTAGE
+ m_timer_battery_voltage_updates = nodeHandle.createTimer(ros::Duration(yaml_battery_polling_period_in_seconds), timerCallback_update_battery_voltage, false);
+ // > And stop it immediately
m_timer_battery_voltage_updates.stop();
+ // INITIALISE TIMER FOR THE STATE ESTIMATE PUBLISHER TIMER
+ m_timer_state_estimate_update = nodeHandle.createTimer(ros::Duration(yaml_cfSimulation_deltaT_in_seconds), timerCallback_update_cfStateEstimate, false);
+ // > And stop it immediately
+ m_timer_state_estimate_update.stop();
+
+
+
+
+ // INITIALISE THE QUADROTOR SIMULATOR
+ // > First convert the agentID to a zero-padded string
+ // Convert the agent ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(2) << std::setfill('0') << m_agentID;
+ std::string agentID_as_string(str_stream.str());
+ // > Initialise the quadrotor simulator class variable
+ m_quadrotor_sim = QuadrotorSimulator( "CF" + agentID_as_string , 0.032 );
+ // Set the agent id of quadrotor simulator
+ // > This determines which message topic the quadrotor
+ // simulator subscribes to for commands
+ m_quadrotor_sim.update_commanding_agent_id( m_agentID );
+ // Set the reset state
+ m_quadrotor_sim.setResetState_xyz_yaw(0.0,0.0,0.0,0.0);
+ // Set the parameters for the 16-bit command to thrust conversion
+ m_quadrotor_sim.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
+ m_quadrotor_sim.reset();
+ // > Inform the user
+ ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] Initilised quadrotor simulation with ID = " << m_quadrotor_sim.get_id_string() << ", and mass in kg = " << m_quadrotor_sim.get_mass_in_kg() );
+ ROS_INFO("[CRAZY RADIO EMULATOR] Initilised quadrotor simulation with details:");
+ m_quadrotor_sim.print_details();
+
+
// PUBLISHERS
@@ -513,12 +681,19 @@ int main(int argc, char* argv[])
// Publisher for the filtered battery voltage
batteryVoltagePublisher = nodeHandle.advertise<std_msgs::Float32>("CFBattery",1);
+ // Publisher for the onboard state estimate
+ cfStateEstimatorPublisher = nodeHandle.advertise<FlyingVehicleState>("CFStateEstimate",1);
+
// Publisher for the control commands
// > Note this is not needed for the real CrazyRadio node because
// this command goes out over the radio, but for emulation we publish
// the control command
controlCommandPublisher = nodeHandle.advertise<ControlCommand>("ControlCommand",1);
+ // Publisher for the simulation state
+ // > This is used by the MocapEmulator node
+ cfSimulationStatePublisher = nodeHandle.advertise<FlyingVehicleState>("CFSimulationState",1);
+
// SUBSCRIBERS
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index cc508b989834143b6e661078d137f67a7e2b62f5..64ffed038626413fb95d1cb51ffa06115c027875 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -82,23 +82,23 @@
// following two properties:
//
// >> request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is
-// defined in the file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+// This property is itself a structure of type "FlyingVehicleState", which is
+// defined in the file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the
// motion capture system of the Crazyflie that is to be controlled by this
// service.
//
// >> request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access
+// This property is an array of "FlyingVehicleState" structures, what allows access
// to the motion capture measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by
@@ -107,39 +107,52 @@
// >> response.ControlCommand
// This property is iteself a structure of type "ControlCommand", which is
// defined in the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines"
-// section in the header file, they are:
-// - CF_COMMAND_TYPE_MOTORS
-// - CF_COMMAND_TYPE_RATE
-// - CF_COMMAND_TYPE_ANGLE
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
//
-// > For most common option to use is CF_COMMAND_TYPE_RATE option.
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
//
-// > For the CF_COMMAND_TYPE_RATE optoin:
-// 1) the ".roll", ".ptich", and ".yaw" properties of
-// "response.ControlCommand" specify the angular rate in
-// [radians/second] that will be requested from the PID controllers
-// running in the Crazyflie 2.0 firmware.
-// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
// "response.ControlCommand" are the baseline motor commands
// requested from the Crazyflie, with the adjustment for body rates
// being added on top of this in the firmware (i.e., as per the
// code of the "distribute_power" found in the firmware).
-// 3) the axis convention for the roll, pitch, and yaw body rates
-// returned in "response.ControlCommand" should use right-hand
-// coordinate axes with x-forward and z-upwards (i.e., the positive
-// z-axis is aligned with the direction of positive thrust). To
-// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -310,12 +323,24 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > THIRD: perform the inner controller
pitchRate_forResponse = 5.0*(pitch_ref-request.ownCrazyflie.pitch);
- // Put the computed body rate commands into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+ // > i.e., put the computed body rate commands into the "response" variable
+
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
// PERFORM THE INTEGRATOR COMPUTATIONS
@@ -335,9 +360,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Put the thrust commands into the "response" variable.
// The thrust adjustment computed by the controller need to be added to the
// the feed-forward thrust that "counter-acts" gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
// > NOTE: the "m_cf_weight_in_newtons" value is the total thrust required
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
@@ -350,16 +374,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor + cmd3_integral_adjustments);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor + cmd4_integral_adjustments);
-
- // PREPARE AND RETURN THE VARIABLE "response"
-
- // Choose the controller type use on-board the Crazyflie,
- // it can be either be Motor, Rate, or Angle based
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
-
-
if (yaml_shouldPublishDebugMessage)
@@ -427,9 +441,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+ ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the per motor 16-bit command computed.
ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
@@ -989,7 +1003,7 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor
// command to thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
// > The min and max for saturating 16 bit thrust commands
yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
@@ -997,7 +1011,7 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
// The default setpoint, the ordering is (x,y,z,yaw),
// with unit [meters,meters,meters,radians]
- getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
// Boolean indiciating whether the "Debug Message" of this agent
// should be published or not
@@ -1009,10 +1023,10 @@ void fetchCsoneControllerYamlParameters(ros::NodeHandle& nodeHandle)
// The LQR Controller parameters
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// Integrator gains
yaml_integratorGain_forThrust = getParameterFloat(nodeHandle_for_paramaters, "integratorGain_forThrust");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 3e6cd6d9bec1c49c03596b4d92bda2d4195cd8e3..a412a8849b18df85af9723d256a8d7d4f1672d0d 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -279,19 +279,25 @@ void computeResponse_for_standby(Controller::Response &response)
m_current_state_changed = false;
}
- // Fill in zero for the angle parts
- response.controlOutput.roll = 0.0;
- response.controlOutput.pitch = 0.0;
- response.controlOutput.yaw = 0.0;
-
+ // Specify that all controllers are disabled
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+
+ // Fill in zero for the controller setpoints
+ response.controlOutput.xControllerSetpoint = 0.0;
+ response.controlOutput.yControllerSetpoint = 0.0;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = 0.0;
+
// Fill in all motor thrusts as zero
response.controlOutput.motorCmd1 = 0.0;
response.controlOutput.motorCmd2 = 0.0;
response.controlOutput.motorCmd3 = 0.0;
response.controlOutput.motorCmd4 = 0.0;
- // Specify that using a "motor type" of command
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+
}
@@ -343,10 +349,17 @@ void computeResponse_for_takeoff_spin_motors(Controller::Response &response)
+ 1000.0
+ time_elapsed_proportion * yaml_takeoff_spin_motors_thrust;
- // Fill in zero for the angle parts
- response.controlOutput.roll = 0.0;
- response.controlOutput.pitch = 0.0;
- response.controlOutput.yaw = 0.0;
+ // Specify that all controllers are disabled
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+
+ // Fill in zero for the controller setpoints
+ response.controlOutput.xControllerSetpoint = 0.0;
+ response.controlOutput.yControllerSetpoint = 0.0;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = 0.0;
// Fill in all motor thrusts as the same
response.controlOutput.motorCmd1 = thrust_for_spinning;
@@ -354,8 +367,6 @@ void computeResponse_for_takeoff_spin_motors(Controller::Response &response)
response.controlOutput.motorCmd3 = thrust_for_spinning;
response.controlOutput.motorCmd4 = thrust_for_spinning;
- // Specify that using a "motor type" of command
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// Change to next state after specified time
if (m_time_in_seconds > yaml_takoff_spin_motors_time)
@@ -672,17 +683,23 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
// Next stop using the controller and just spin the motors
else
{
- // Fill in zero for the angle parts
- response.controlOutput.roll = 0.0;
- response.controlOutput.pitch = 0.0;
- response.controlOutput.yaw = 0.0;
+ // Specify that all controllers are disabled
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+
+ // Fill in zero for the controller setpoints
+ response.controlOutput.xControllerSetpoint = 0.0;
+ response.controlOutput.yControllerSetpoint = 0.0;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = 0.0;
+
// Fill in all motor thrusts as the same
response.controlOutput.motorCmd1 = yaml_landing_spin_motors_thrust;
response.controlOutput.motorCmd2 = yaml_landing_spin_motors_thrust;
response.controlOutput.motorCmd3 = yaml_landing_spin_motors_thrust;
response.controlOutput.motorCmd4 = yaml_landing_spin_motors_thrust;
- // Specify that using a "motor type" of command
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
// Change to next state after specified time
if ( m_time_in_seconds > (0.7*yaml_landing_spin_motors_time) )
@@ -782,73 +799,97 @@ void smoothSetpointChanges( float target_setpoint[4] , float (¤t_setpoint)
void performEstimatorUpdate_forStateInterial(Controller::Request &request)
{
- // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
- // > for (x,y,z) position
- m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
- m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
- m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
- // > for (roll,pitch,yaw) angles
- m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
- m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
- m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+ if (request.ownCrazyflie.type == FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT)
+ {
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ m_current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ m_current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ m_current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ m_current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ m_current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ m_current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
- // RUN THE FINITE DIFFERENCE ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
+ // RUN THE FINITE DIFFERENCE ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(request.isFirstControllerCall);
- // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
- performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(request.isFirstControllerCall);
- // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
- switch (yaml_estimator_method)
- {
- // Estimator based on finte differences
- case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (yaml_estimator_method)
{
- // Transfer the estimate
- for(int i = 0; i < 9; ++i)
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
{
- m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
+ // Transfer the estimate
+ for(int i = 0; i < 9; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
}
- break;
- }
- // Estimator based on Point Mass Kalman Filter
- case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
- {
- // Transfer the estimate
- for(int i = 0; i < 9; ++i)
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
{
- m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ // Transfer the estimate
+ for(int i = 0; i < 9; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
}
- break;
- }
- // Handle the exception
- default:
- {
- // Display that the "yaml_estimator_method" was not recognised
- ROS_INFO_STREAM("[DEFAULT CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DefaultControllerService': the 'yaml_estimator_method' is not recognised.");
- // Transfer the finite difference estimate by default
- for(int i = 0; i < 9; ++i)
+ // Handle the exception
+ default:
{
- m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
+ // Display that the "yaml_estimator_method" was not recognised
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DefaultControllerService': the 'yaml_estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 9; ++i)
+ {
+ m_current_stateInertialEstimate[i] = m_stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
}
- break;
}
- }
- // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
- // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
- // > for (x,y,z) position
- m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
- m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
- m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
- // > for (roll,pitch,yaw) angles
- m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
- m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
- m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ m_previous_xzy_rpy_measurement[0] = m_current_xzy_rpy_measurement[0];
+ m_previous_xzy_rpy_measurement[1] = m_current_xzy_rpy_measurement[1];
+ m_previous_xzy_rpy_measurement[2] = m_current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ m_previous_xzy_rpy_measurement[3] = m_current_xzy_rpy_measurement[3];
+ m_previous_xzy_rpy_measurement[4] = m_current_xzy_rpy_measurement[4];
+ m_previous_xzy_rpy_measurement[5] = m_current_xzy_rpy_measurement[5];
+ }
+ else if (request.ownCrazyflie.type == FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE)
+ {
+ // Transfer the onboard estimate directly
+ // > For position
+ m_current_stateInertialEstimate[0] = request.ownCrazyflie.x;
+ m_current_stateInertialEstimate[1] = request.ownCrazyflie.y;
+ m_current_stateInertialEstimate[2] = request.ownCrazyflie.z;
+ // > For velocities
+ m_current_stateInertialEstimate[3] = request.ownCrazyflie.vx;
+ m_current_stateInertialEstimate[4] = request.ownCrazyflie.vy;
+ m_current_stateInertialEstimate[5] = request.ownCrazyflie.vz;
+ // > For euler angles
+ m_current_stateInertialEstimate[6] = request.ownCrazyflie.roll;
+ m_current_stateInertialEstimate[7] = request.ownCrazyflie.pitch;
+ m_current_stateInertialEstimate[8] = request.ownCrazyflie.yaw;
+ }
+ else
+ {
+ ROS_ERROR_STREAM("[DEFAULT CONTROLLER] Received a request.ownCrazyflie with unrecognised type, request.ownCrazyflie.type = " << request.ownCrazyflie.type );
+ }
}
@@ -870,9 +911,9 @@ void performEstimatorUpdate_forStateInterial_viaFiniteDifference(bool isFirstUpd
if (!isFirstUpdate)
{
// > for (x,y,z) velocities
- m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * m_estimator_frequency;
- m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * m_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[3] = (m_current_xzy_rpy_measurement[0] - m_previous_xzy_rpy_measurement[0]) * yaml_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[4] = (m_current_xzy_rpy_measurement[1] - m_previous_xzy_rpy_measurement[1]) * yaml_estimator_frequency;
+ m_stateInterialEstimate_viaFiniteDifference[5] = (m_current_xzy_rpy_measurement[2] - m_previous_xzy_rpy_measurement[2]) * yaml_estimator_frequency;
}
else
{
@@ -1124,15 +1165,23 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0 + newtons_int;
@@ -1142,11 +1191,6 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor + tau_x + tau_y - tau_z);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor + tau_x - tau_y + tau_z);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// An alternate debugging technique is to print out data directly to the
@@ -1155,9 +1199,9 @@ void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controlle
{
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+ ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
@@ -1365,8 +1409,8 @@ void setNewSetpoint(float x, float y, float z, float yaw)
{
// Put the new setpoint into the class variable
m_setpoint[0] = clipToBounds( x , -m_symmetric_area_bounds_x , m_symmetric_area_bounds_x );
- m_setpoint[1] = clipToBounds( y , -m_symmetric_area_bounds_y , m_symmetric_area_bounds_y );;
- m_setpoint[2] = clipToBounds( z , -m_symmetric_area_bounds_z , m_symmetric_area_bounds_z );;
+ m_setpoint[1] = clipToBounds( y , -m_symmetric_area_bounds_y , m_symmetric_area_bounds_y );
+ m_setpoint[2] = clipToBounds( z , m_area_bounds_zmin , m_area_bounds_zmax );
m_setpoint[3] = yaw;
}
@@ -1555,47 +1599,31 @@ void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
void loadCrazyflieContext()
{
- CMQuery contextCall;
- contextCall.request.studentID = m_agentID;
- ROS_INFO_STREAM("[DEFAULT CONTROLLER] AgentID:" << m_agentID);
-
- CrazyflieContext new_context;
-
- m_centralManager.waitForExistence(ros::Duration(-1));
-
- if(m_centralManager.call(contextCall)) {
- new_context = contextCall.response.crazyflieContext;
- //ROS_INFO_STREAM("[DEFAULT CONTROLLER] CrazyflieContext:\n" << new_context);
-
- //if((m_context.crazyflieName != "") && (new_context.crazyflieName != m_context.crazyflieName)) //linked crazyflie name changed and it was not empty before
- //{
+ CMQuery contextCall;
+ contextCall.request.studentID = m_agentID;
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] AgentID:" << m_agentID);
- // Motors off is done in python script now everytime we disconnect
+ CrazyflieContext new_context;
- // send motors OFF and disconnect before setting m_context = new_context
- // std_msgs::Int32 msg;
- // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
- // commandPublisher.publish(msg);
+ m_centralManager.waitForExistence(ros::Duration(-1));
- // ROS_INFO("[DEFAULT CONTROLLER] CF is now different for this student. Disconnect and turn it off");
+ if(m_centralManager.call(contextCall)) {
+ new_context = contextCall.response.crazyflieContext;
- // IntWithHeader msg;
- // msg.shouldCheckForAgentID = false;
- // msg.data = CMD_DISCONNECT;
- // m_crazyRadioCommandPublisher.publish(msg);
- //}
+ m_symmetric_area_bounds_x = 0.5 * (new_context.localArea.xmax - new_context.localArea.xmin);
+ m_symmetric_area_bounds_y = 0.5 * (new_context.localArea.ymax - new_context.localArea.ymin);
+ //m_symmetric_area_bounds_z = 0.5 * (new_context.localArea.zmax - new_context.localArea.zmin);
- m_symmetric_area_bounds_x = 0.5 * (new_context.localArea.xmax - new_context.localArea.xmin);
- m_symmetric_area_bounds_y = 0.5 * (new_context.localArea.ymax - new_context.localArea.ymin);
- m_symmetric_area_bounds_z = 0.5 * (new_context.localArea.zmax - new_context.localArea.zmin);
- m_isAvailableContext = true;
+ m_area_bounds_zmin = new_context.localArea.zmin;
+ m_area_bounds_zmax = new_context.localArea.zmax;
- }
- else
- {
- m_isAvailableContext = false;
- ROS_ERROR("[DEFAULT CONTROLLER] Failed to load context. Waiting for next Save in DB by System Config node");
- }
+ m_isAvailableContext = true;
+ }
+ else
+ {
+ m_isAvailableContext = false;
+ ROS_ERROR("[DEFAULT CONTROLLER] Failed to load context. Waiting for next Save in DB by System Config node");
+ }
}
@@ -1782,7 +1810,7 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit
// motor command to thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
// The min and max for saturating 16 bit thrust commands
yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
@@ -1790,7 +1818,7 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// The default setpoint, the ordering is (x,y,z,yaw),
// with unit [meters,meters,meters,radians]
- getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
// Boolean indiciating whether the "Debug Message" of this agent should be published or not
yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
@@ -1802,13 +1830,13 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_controller_method = getParameterInt( nodeHandle_for_paramaters , "controller_method" );
// The LQR Controller parameters for z-height
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_2StateVector", yaml_gainMatrixThrust_2StateVector, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_2StateVector", yaml_gainMatrixThrust_2StateVector, 2);
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_3StateVector", yaml_gainMatrixRollRate_3StateVector, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_3StateVector", yaml_gainMatrixPitchRate_3StateVector, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_3StateVector", yaml_gainMatrixRollRate_3StateVector, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_3StateVector", yaml_gainMatrixPitchRate_3StateVector, 3);
// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_2StateVector", yaml_gainMatrixRollAngle_2StateVector, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_2StateVector", yaml_gainMatrixPitchAngle_2StateVector, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle_2StateVector", yaml_gainMatrixRollAngle_2StateVector, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle_2StateVector", yaml_gainMatrixPitchAngle_2StateVector, 2);
yaml_gainRollRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainRollRate_fromAngle");
yaml_gainPitchRate_fromAngle = getParameterFloat(nodeHandle_for_paramaters, "gainPitchRate_fromAngle");
// The LQR Controller parameters for yaw
@@ -1821,12 +1849,14 @@ void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle)
// A flag for which estimator to use:
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
+
+ yaml_estimator_frequency = getParameterFloat(nodeHandle_for_paramaters, "estimator_frequency");
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
index 7988825d0e6b7b62c58a5b4f8ff217bb0ad575ee..e83ad504ffa021e70db23c615178024a9dfb349c 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DemoControllerService.cpp
@@ -80,22 +80,22 @@
// The arument "request" is a structure provided to this service with the following two
// properties:
// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+// This property is itself a structure of type "FlyingVehicleState", which is defined in the
+// file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
+// This property is an array of "FlyingVehicleState" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
@@ -1526,7 +1526,7 @@ void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paraMaters, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paraMaters, "shouldPerformConvertIntoBodyFrame");
@@ -1565,36 +1565,36 @@ void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
estimator_method = getParameterInt( nodeHandle_for_paraMaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_MOTOR"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor1", gainMatrixMotor1, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor2", gainMatrixMotor2, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor3", gainMatrixMotor3, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixMotor4", gainMatrixMotor4, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor1", gainMatrixMotor1, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor2", gainMatrixMotor2, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor3", gainMatrixMotor3, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixMotor4", gainMatrixMotor4, 12);
// The LQR Controller parameters for "LQR_MODE_MOTOR"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paraMaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// The parameters for the "Angle Reponse Test" controller mode
angleResponseTest_pitchAngle_degrees = getParameterFloat(nodeHandle_for_paraMaters, "angleResponseTest_pitchAngle_degrees");
@@ -1607,13 +1607,13 @@ void fetchDemoControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paraMaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paraMaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 63445d1be767f07bddde21d22fa206d891c92277..6f4b4aa03591366cf3670e8b9669880287025592 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -59,7 +59,6 @@
-
// ----------------------------------------------------------------------------------
// M M OOO TTTTT III OOO N N
// MM MM O O T I O O NN N
@@ -81,12 +80,9 @@ void viconCallback(const ViconData& viconData)
// CAPTURE SYSTEM. HENCE ANY OPTERATIONS PERFORMED IN
// THIS FUNCTION MUST BE NON-BLOCKING.
- // Initialise a counter of consecutive frames of motion
- // capture data where the agent is occuled
- static int number_of_consecutive_occulsions = 0;
-
// Initilise a variable for the pose data of this agent
- CrazyflieData poseDataForThisAgent;
+ FlyingVehicleState poseDataForThisAgent;
+ poseDataForThisAgent.isValid = false;
// Extract the pose data for the allocated object from the
// full motion capture array
@@ -94,9 +90,11 @@ void viconCallback(const ViconData& viconData)
// indicates that the pose data was not found.
m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
+
+
// Initilise a variable for the pose data of another agent
- CrazyflieData poseDataForOtherAgent;
- poseDataForOtherAgent.occluded = true;
+ FlyingVehicleState poseDataForOtherAgent;
+ poseDataForOtherAgent.isValid = false;
// Extract the pose data for the other object from the
// full motion capture array
@@ -104,73 +102,231 @@ void viconCallback(const ViconData& viconData)
// indicates that the pose data was not found.
//m_otherObjectPoseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, "NameOfOtherObject" , m_otherObjectPoseDataIndex , poseDataForOtherAgent );
- // Detecting time-out of the motion capture data
- // > Update the flag
- m_isAvailable_mocap_data = true;
- // > Stop any previous instance that might still be running
- m_timer_mocap_timeout_check.stop();
- // > Set the period again (second argument is reset)
- m_timer_mocap_timeout_check.setPeriod( ros::Duration(yaml_mocap_timeout_duration), true);
- // > Start the timer again
- m_timer_mocap_timeout_check.start();
// Only continue if:
- // (1) the pose for this agent was found, and
- // (2) the controllers are actually available
- // (2) the flying state is something other than MOTORS-OFF
+ // (1) the pose for this agent was found
+ if (m_poseDataIndex >= 0)
+ {
+ callControllerServiceWithGivenData(poseDataForThisAgent, poseDataForOtherAgent);
+ }
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ }
+}
+
+
+
+
+
+int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , FlyingVehicleState& pose_to_fill_in)
+{
+ // Initialise an integer for the index where the object
+ // "name" is found
+ // > Initialise an negative to indicate not found
+ int object_index = -1;
+
+ // Get the length of the "pose data array"
+ int length_poseData = viconData.crazyflies.size();
+
+ // If the "expected index" is non-negative and less than
+ // the length of the data array, then attempt to check
+ // for a name match
if (
- (m_poseDataIndex >= 0)
+ (0 <= expected_index)
and
- (m_controllers_avialble)
+ (expected_index < length_poseData)
)
{
+ // Check if the names match
+ if (viconData.crazyflies[expected_index].vehicleName == name)
+ {
+ object_index = expected_index;
+ }
+ }
- // If motors-off and testing motors
- if (
- m_flying_state == STATE_MOTORS_OFF
- &&
- m_controller_nominally_selected == TESTMOTORS_CONTROLLER
- &&
- m_testMotorsController.exists()
- )
- {
- // Initliase the "Contrller" service call variable
- Controller testMotorsCall;
+ // If not found, then iterate the data array looking
+ // for a name match
+ if (object_index < 0)
+ {
+ for( int i=0 ; i<length_poseData ; i++ )
+ {
+ // Check if the names match
+ if(viconData.crazyflies[i].vehicleName == name)
+ {
+ object_index = i;
+ }
+ }
+ }
- // Initialise a local boolean success variable
- bool isSuccessful_testMotorsCall = false;
+ // If not found, then retrun, else fill in the pose data
+ if (object_index < 0)
+ {
+ return object_index;
+ }
+ else
+ {
+ pose_to_fill_in = viconData.crazyflies[object_index];
+ coordinatesToLocal(pose_to_fill_in);
+ return object_index;
+ }
+}
- // Call the controller service client
- isSuccessful_testMotorsCall = m_testMotorsController.call(testMotorsCall);
- // Ensure success and enforce safety
- if(isSuccessful_testMotorsCall)
- {
- m_commandForSendingToCrazyfliePublisher.publish(testMotorsCall.response.controlOutput);
- }
- else
- {
- // Let the user know that the controller call failed
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call test motors controller, valid: " << m_testMotorsController.isValid() << ", exists: " << m_testMotorsController.exists());
- // Change back to the default controller
- setControllerNominallySelected(DEFAULT_CONTROLLER);
- // Send the command to turn the motors off
- sendZeroOutputCommandForMotors();
- }
- return;
- }
+void coordinatesToLocal(FlyingVehicleState& cf)
+{
+ // Get the area into a local variable
+ AreaBounds area = m_context.localArea;
+
+ // Shift the X-Y coordinates
+ float originX = (area.xmin + area.xmax) / 2.0;
+ float originY = (area.ymin + area.ymax) / 2.0;
+ cf.x -= originX;
+ cf.y -= originY;
+
+ // Shift the Z coordinate
+ // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
+ //float originZ = 0.0;
+ // float originZ = (area.zmin + area.zmax) / 2.0;
+ //cf.z -= originZ;
+}
+
+
+
+
+
+// ---------------------------------------------------------------------------- %
+// OOO N N BBBB OOO A RRRR DDDD
+// O O NN N B B O O A A R R D D
+// O O N N N BBBB O O A A RRRR D D
+// O O N NN B B O O AAAAA R R D D
+// OOO N N BBBB OOO A A R R DDDD
+//
+// EEEEE SSSS TTTTT III M M A TTTTT EEEEE
+// E S T I MM MM A A T E
+// EEE SSS T I M M M A A T EEE
+// E S T I M M AAAAA T E
+// EEEEE SSSS T III M M A A T EEEEE
+// ---------------------------------------------------------------------------- %
+
+// FUNCTIONS FOR HANDLING THE ONBOARD STATE ESTIMATE
+// > Callback run every time new state estimate
+// data is available from onboard the flying vehicle
+void onboardEstimateCallback(const FlyingVehicleState& flyingVehicleState)
+{
+ // Pass the receive data directly through
+ // > Copy into a local vairable first
+ FlyingVehicleState dataForThisAgent;
+ dataForThisAgent = flyingVehicleState;
+
+ // Initialise an empty variable for the "other
+ // agent data" argument
+ FlyingVehicleState dataForOtherAgent;
+ dataForOtherAgent.isValid = false;
+
+ // Call the "call controller service" function
+ callControllerServiceWithGivenData( dataForThisAgent , dataForOtherAgent );
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC A L L
+// C A A L L
+// C A A L L
+// C AAAAA L L
+// CCCC A A LLLLL LLLLL
+//
+// CCCC OOO N N TTTTT RRRR OOO L L EEEEE RRRR
+// C O O NN N T R R O O L L E R R
+// C O O N N N T RRRR O O L L EEE RRRR
+// C O O N NN T R R O O L L E R R
+// CCCC OOO N N T R R OOO LLLLL LLLLL EEEEE R R
+//
+// SSSS EEEEE RRRR V V III CCCC EEEEE
+// S E R R V V I C E
+// SSS EEE RRRR V V I C EEE
+// S E R R V V I C E
+// SSSS EEEEE R R V III CCCC EEEEE
+// ----------------------------------------------------------------------------------
+
+// FUNCTION FOR CALLING THE CONTROLLER SERVICE FOR THE
+// CURRENTLY SELECTED CONTROLLER, AND THEN SENDING THE
+// CONTROL COMMAND TO THE CRAZYRADIO NODE
+void callControllerServiceWithGivenData(FlyingVehicleState& dataForThisAgent, FlyingVehicleState& dataForOtherAgent)
+{
+
+ // Initialise a counter of consecutive frames of motion
+ // capture data where the agent is occuled
+ static int number_of_consecutive_invalid_measurements = 0;
+
+ // Detecting time-out of the measurement data
+ // > Update the flag
+ m_isAvailable_measurement_data = true;
+ // > Stop any previous instance that might still be running
+ m_timer_measurement_data_timeout_check.stop();
+ // > Set the period again (second argument is reset)
+ m_timer_measurement_data_timeout_check.setPeriod( ros::Duration(yaml_measurement_timeout_duration), true);
+ // > Start the timer again
+ m_timer_measurement_data_timeout_check.start();
+
+ // Only continue if:
+ // (1) the controllers are actually available
+ if (m_controllers_avialble)
+ {
+
+ // Call the "test motors" controller only if:
+ // (1) in the motors-off state, and
+ // (2) "test motors" controller is selected, and
+ // (3) "test motors" controller exists
+ if (
+ m_flying_state == STATE_MOTORS_OFF
+ &&
+ m_controller_nominally_selected == TESTMOTORS_CONTROLLER
+ &&
+ m_testMotorsController.exists()
+ )
+ {
+ // Initliase the "Contrller" service call variable
+ Controller testMotorsCall;
+
+ // Initialise a local boolean success variable
+ bool isSuccessful_testMotorsCall = false;
+
+ // Call the controller service client
+ isSuccessful_testMotorsCall = m_testMotorsController.call(testMotorsCall);
+
+ // Ensure success and enforce safety
+ if(isSuccessful_testMotorsCall)
+ {
+ m_commandForSendingToCrazyfliePublisher.publish(testMotorsCall.response.controlOutput);
+ }
+ else
+ {
+ // Let the user know that the controller call failed
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call test motors controller, valid: " << m_testMotorsController.isValid() << ", exists: " << m_testMotorsController.exists());
+ // Change back to the default controller
+ setControllerNominallySelected(DEFAULT_CONTROLLER);
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ }
+ return;
+ }
// Only continue if:
- // (1) the agent is NOT occulded
- if(!poseDataForThisAgent.occluded)
+ // (1) the agent's pose data is valid
+ if(dataForThisAgent.isValid)
{
// Update the flag
- m_isOcculded_mocap_data = false;
+ m_isValid_measurement_data = true;
// Reset the "consecutive occulsions" flag
- number_of_consecutive_occulsions = 0;
+ number_of_consecutive_invalid_measurements = 0;
// Only continue if:
@@ -182,13 +338,13 @@ void viconCallback(const ViconData& viconData)
Controller controllerCall;
// Fill in the pose data for this agent
- controllerCall.request.ownCrazyflie = poseDataForThisAgent;
+ controllerCall.request.ownCrazyflie = dataForThisAgent;
// Fill in the pose data of another agaent, if the data
// is avaialable
- if (m_otherObjectPoseDataIndex >= 0)
+ if (dataForOtherAgent.isValid)
{
- controllerCall.request.otherCrazyflies.push_back(poseDataForOtherAgent);
+ controllerCall.request.otherCrazyflies.push_back(dataForOtherAgent);
}
// Fill in the "isFirstControllerCall" field
@@ -206,7 +362,7 @@ void viconCallback(const ViconData& viconData)
// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
if ( getInstantController() != DEFAULT_CONTROLLER )
{
- if ( !safetyCheck_on_positionAndTilt(poseDataForThisAgent) )
+ if ( !safetyCheck_on_positionAndTilt(dataForThisAgent) )
{
setInstantController(DEFAULT_CONTROLLER);
ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
@@ -268,24 +424,24 @@ void viconCallback(const ViconData& viconData)
else
{
// Increment the number of consective occulations
- number_of_consecutive_occulsions++;
+ number_of_consecutive_invalid_measurements++;
// Update the flag if this exceeds the threshold
if (
- (!m_isOcculded_mocap_data)
+ (m_isValid_measurement_data)
and
- (number_of_consecutive_occulsions > yaml_consecutive_occulsions_threshold)
+ (number_of_consecutive_invalid_measurements > yaml_consecutive_invalid_threshold)
)
{
// Update the flag
- m_isOcculded_mocap_data = true;
+ m_isValid_measurement_data = false;
// Inform the user
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Agent was occluded for more than " << yaml_consecutive_occulsions_threshold << " consecutive frames." );
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Measurement data was invalid for more than " << yaml_consecutive_invalid_threshold << " consecutive measurements." );
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
// Update the flying state
requestChangeFlyingStateTo( STATE_MOTORS_OFF );
}
- } // END OF: "if(!poseDataForThisAgent.occluded)"
+ } // END OF: "if(dataForThisAgent.isValid)"
}
else
@@ -293,112 +449,56 @@ void viconCallback(const ViconData& viconData)
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
- } // END OF: "if ( (m_poseDataIndex >= 0) and (m_controllers_avialble) )"
-
-}
+ } // END OF: "if (m_controllers_avialble)"
+}
-int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose_to_fill_in)
+// FUNCTION FOR SENDING A COMMAND, VIA THE CRAZYRADIO, THAT
+// THE MOTORS SHOULD BE SET TO ZERO
+void sendZeroOutputCommandForMotors()
{
- // Initialise an integer for the index where the object
- // "name" is found
- // > Initialise an negative to indicate not found
- int object_index = -1;
-
- // Get the length of the "pose data array"
- int length_poseData = viconData.crazyflies.size();
-
- // If the "expected index" is non-negative and less than
- // the length of the data array, then attempt to check
- // for a name match
- if (
- (0 <= expected_index)
- and
- (expected_index < length_poseData)
- )
- {
- // Check if the names match
- if (viconData.crazyflies[expected_index].crazyflieName == name)
- {
- object_index = expected_index;
- }
- }
-
- // If not found, then iterate the data array looking
- // for a name match
- if (object_index < 0)
- {
- for( int i=0 ; i<length_poseData ; i++ )
- {
- // Check if the names match
- if(viconData.crazyflies[i].crazyflieName == name)
- {
- object_index = i;
- }
- }
- }
-
- // If not found, then retrun, else fill in the pose data
- if (object_index < 0)
- {
- return object_index;
- }
- else
- {
- pose_to_fill_in = viconData.crazyflies[object_index];
- coordinatesToLocal(pose_to_fill_in);
- return object_index;
- }
+ // Initialise a "control command" struct
+ // > Note that all value are set to zero by default
+ ControlCommand zeroOutput = ControlCommand();
+ // To be sure, set the controller mode and motor command anyway
+ // > Specify that all controllers are disabled
+ zeroOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ zeroOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ zeroOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ zeroOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ // Fill in all motor thrusts as zero
+ zeroOutput.motorCmd1 = 0.0;
+ zeroOutput.motorCmd2 = 0.0;
+ zeroOutput.motorCmd3 = 0.0;
+ zeroOutput.motorCmd4 = 0.0;
+ // Publish the command
+ m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
}
-void coordinatesToLocal(CrazyflieData& cf)
-{
- // Get the area into a local variable
- AreaBounds area = m_context.localArea;
-
- // Shift the X-Y coordinates
- float originX = (area.xmin + area.xmax) / 2.0;
- float originY = (area.ymin + area.ymax) / 2.0;
- cf.x -= originX;
- cf.y -= originY;
-
- // Shift the Z coordinate
- // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
- //float originZ = 0.0;
- // float originZ = (area.zmin + area.zmax) / 2.0;
- //cf.z -= originZ;
-}
-
-void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
+// CALLBACK FUNCTION RUN WHEN MEASUREMENT DATA HAS
+// NOT BEEN RECEIVED FOR A SPECIFIED TIME
+void timerCallback_measurement_data_timeout_check(const ros::TimerEvent&)
{
// Update the flag
- m_isAvailable_mocap_data = false;
+ m_isAvailable_measurement_data = false;
// Inform the user
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Motion Capture Data has been unavailable for " << yaml_mocap_timeout_duration << " seconds." );
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Measurement data has been unavailable for " << yaml_measurement_timeout_duration << " seconds." );
// Ensure that the motors are turned off
if ( !(m_flying_state==STATE_MOTORS_OFF) )
{
// Send the command to turn the motors off
- sendZeroOutputCommandForMotors();
- // Update the flying state
- requestChangeFlyingStateTo( STATE_MOTORS_OFF );
+ sendZeroOutputCommandForMotors();
+ // Update the flying state
+ requestChangeFlyingStateTo( STATE_MOTORS_OFF );
}
}
-void sendZeroOutputCommandForMotors()
-{
- ControlCommand zeroOutput = ControlCommand(); //everything set to zero
- zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
- m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
-}
-
-
@@ -417,62 +517,62 @@ void sendZeroOutputCommandForMotors()
// ----------------------------------------------------------------------------------
// Checks if crazyflie is within allowed area
-bool safetyCheck_on_positionAndTilt(CrazyflieData data)
+bool safetyCheck_on_positionAndTilt(FlyingVehicleState data)
{
- // Check on the X position
- float symmetric_bound_x = 0.5 * (m_context.localArea.xmax-m_context.localArea.xmin);
- if((data.x < -symmetric_bound_x) or (data.x > symmetric_bound_x))
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
- return false;
- }
- // Check on the Y position
- float symmetric_bound_y = 0.5 * (m_context.localArea.ymax-m_context.localArea.ymin);
- if((data.y < -symmetric_bound_y) or (data.y > symmetric_bound_y))
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
- return false;
- }
- // Check on the Z position
- if((data.z < m_context.localArea.zmin) or (data.z > m_context.localArea.zmax))
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] z safety failed");
- return false;
- }
+ // Check on the X position
+ float symmetric_bound_x = 0.5 * (m_context.localArea.xmax-m_context.localArea.xmin);
+ if((data.x < -symmetric_bound_x) or (data.x > symmetric_bound_x))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
+ return false;
+ }
+ // Check on the Y position
+ float symmetric_bound_y = 0.5 * (m_context.localArea.ymax-m_context.localArea.ymin);
+ if((data.y < -symmetric_bound_y) or (data.y > symmetric_bound_y))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
+ return false;
+ }
+ // Check on the Z position
+ if((data.z < m_context.localArea.zmin) or (data.z > m_context.localArea.zmax))
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] z safety failed");
+ return false;
+ }
- // Check the title angle (if required)
- // > The tilt anlge between the body frame and inertial frame z-axis is
- // give by:
- // tilt angle = 1 / ( cos(roll)*cos(pitch) )
- // > But this would be too sensitve to a divide by zero error, so instead
- // we just check if each angle separately exceeds the limit
- if(yaml_isEnabled_strictSafety)
- {
- // Check on the ROLL angle
- if(
- (data.roll > yaml_maxTiltAngle_for_strictSafety_radians)
- or
- (data.roll < -yaml_maxTiltAngle_for_strictSafety_radians)
- )
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
- return false;
- }
- // Check on the PITCH angle
- if(
- (data.pitch > yaml_maxTiltAngle_for_strictSafety_radians)
- or
- (data.pitch < -yaml_maxTiltAngle_for_strictSafety_radians)
- )
- {
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
- return false;
- }
- }
+ // Check the title angle (if required)
+ // > The tilt anlge between the body frame and inertial frame z-axis is
+ // give by:
+ // tilt angle = 1 / ( cos(roll)*cos(pitch) )
+ // > But this would be too sensitve to a divide by zero error, so instead
+ // we just check if each angle separately exceeds the limit
+ if(yaml_isEnabled_strictSafety)
+ {
+ // Check on the ROLL angle
+ if(
+ (data.roll > yaml_maxTiltAngle_for_strictSafety_radians)
+ or
+ (data.roll < -yaml_maxTiltAngle_for_strictSafety_radians)
+ )
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
+ return false;
+ }
+ // Check on the PITCH angle
+ if(
+ (data.pitch > yaml_maxTiltAngle_for_strictSafety_radians)
+ or
+ (data.pitch < -yaml_maxTiltAngle_for_strictSafety_radians)
+ )
+ {
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
+ return false;
+ }
+ }
- // If the code makes it to here then all the safety checks passed,
- // Hence return "true"
- return true;
+ // If the code makes it to here then all the safety checks passed,
+ // Hence return "true"
+ return true;
}
@@ -560,7 +660,7 @@ void requestChangeFlyingStateToTakeOff()
else
{
// Check that the Motion Capture data is available
- if ( m_isAvailable_mocap_data and !m_isOcculded_mocap_data )
+ if ( m_isAvailable_measurement_data and m_isValid_measurement_data )
{
// Check whether a "controller" take-off should
// be performed, and that not already in the
@@ -614,13 +714,13 @@ void requestChangeFlyingStateToTakeOff()
else
{
// Inform the user of the problem
- if (!m_isAvailable_mocap_data)
+ if (!m_isAvailable_measurement_data)
{
- ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the motion capture data is unavailable.");
+ ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the measurement data is unavailable.");
}
- if (m_isOcculded_mocap_data)
+ if (!m_isValid_measurement_data)
{
- ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the object is \"long-term\" occuled.");
+ ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the object measurements are \"long-term\" invalid.");
}
}
}
@@ -1217,7 +1317,8 @@ void loadCrazyflieContext()
m_context = new_context;
ros::NodeHandle nh("CrazyRadio");
- nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
+ nh.setParam("crazyflieAddress", m_context.crazyflieAddress);
+ nh.setParam("crazyflieName", m_context.crazyflieName);
}
else
{
@@ -1394,22 +1495,26 @@ void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg)
// > Load the paramters from the Client Config YAML file
void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the file:
- // FlyingAgentClientConfig.yaml
+ // Here we load the parameters that are specified in the file:
+ // FlyingAgentClientConfig.yaml
- // Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
+ // Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
- // Flag for whether to use angle for switching to the Safe Controller
- yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
- yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
+ // Flags for which measurement streams to use
+ yaml_shouldUse_mocapMeasurements = getParameterBool(nodeHandle_for_paramaters,"shouldUse_mocapMeasurements");
+ yaml_shouldUse_onboardEstimate = getParameterBool(nodeHandle_for_paramaters,"shouldUse_onboardEstimate");
- // Number of consecutive occulsions that will deem the
- // object as "long-term" occuled
- yaml_consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");
+ // Flag for whether to use angle for switching to the Safe Controller
+ yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
+ yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
+
+ // Number of consecutive occulsions that will deem the
+ // object as "long-term" occuled
+ yaml_consecutive_invalid_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_invalid_threshold");
// Time out duration after which Mocap is considered unavailable
- yaml_mocap_timeout_duration = getParameterFloat(nodeHandle_for_paramaters,"mocap_timeout_duration");
+ yaml_measurement_timeout_duration = getParameterFloat(nodeHandle_for_paramaters,"measurement_timeout_duration");
// Flag for whether the take-off should be performed
// with the default controller
@@ -1418,18 +1523,65 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
// Flag for whether the landing should be performed
// with the default controller
yaml_shouldPerfom_landing_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_landing_with_defaultController");
-
- // DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
- // PROCESS THE PARAMTERS
- // Convert from degrees to radians
- yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
- // DEBUGGING: Print out one of the processed values
- ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
+ // PROCESS THE PARAMTERS
+ // Convert from degrees to radians
+ yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
+
+ // Adjust the measurement subscribers
+ // > Localisation data from motion capture
+ if (yaml_shouldUse_mocapMeasurements)
+ {
+ // > Create a node handle to the root of the D-FaLL system
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+ // > The second argument is the queue_size, and the
+ // documentation states that:
+ // "If messages are arriving too fast and you are
+ // unable to keep up, roscpp will start throwing
+ // away messages."
+ // > It is clearly stated that publisher queues throw
+ // away old messages first, and subscriber queues
+ // are likely the same.
+ // > As we are only interested in latest measurement
+ // a short queue is used.
+ viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 3, viconCallback);
+ }
+ else
+ {
+ viconSubscriber.shutdown();
+ }
+
+ // > Localisation from onboard the flying vehicle
+ if (yaml_shouldUse_onboardEstimate)
+ {
+ // > Create a node handle for the Crazyradio
+ std::string m_namespace = ros::this_node::getNamespace();
+ std::string namespace_to_crazy_radio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_crazy_radio(namespace_to_crazy_radio);
+ // > The second argument is the queue_size, and the
+ // documentation states that:
+ // "If messages are arriving too fast and you are
+ // unable to keep up, roscpp will start throwing
+ // away messages."
+ // > It is clearly stated that publisher queues throw
+ // away old messages first, and subscriber queues
+ // are likely the same.
+ // > As we are only interested in latest measurement
+ // a short queue is used.
+ onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 3, onboardEstimateCallback);
+ }
+ else
+ {
+ onboardEstimateSubscriber.shutdown();
+ }
+
+ // DEBUGGING: Print out one of the processed values
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
}
@@ -1563,9 +1715,9 @@ int main(int argc, char* argv[])
// INITIALISE THE MOTION CAPTURE TIME-OUT TIMER
// > And stop it immediately
- m_isAvailable_mocap_data = false;
- m_timer_mocap_timeout_check = nodeHandle.createTimer(ros::Duration(yaml_mocap_timeout_duration), timerCallback_mocap_timeout_check, true);
- m_timer_mocap_timeout_check.stop();
+ m_isAvailable_measurement_data = false;
+ m_timer_measurement_data_timeout_check = nodeHandle.createTimer(ros::Duration(yaml_measurement_timeout_duration), timerCallback_measurement_data_timeout_check, true);
+ m_timer_measurement_data_timeout_check.stop();
@@ -1602,6 +1754,10 @@ int main(int argc, char* argv[])
constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+ // CREATE A NODE HANDLE FOR THE CRAZYRADIO
+ std::string namespace_to_crazy_radio = m_namespace + "/CrazyRadio";
+ ros::NodeHandle nodeHandle_to_crazy_radio(namespace_to_crazy_radio);
+
// LOADING OF THIS AGENT'S CONTEXT
// Service cleint for loading the allocated flying
@@ -1620,13 +1776,41 @@ int main(int argc, char* argv[])
// LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
- //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
- ros::Subscriber viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, viconCallback);
+ // > The second argument is the queue_size, and the
+ // documentation states that:
+ // "If messages are arriving too fast and you are
+ // unable to keep up, roscpp will start throwing
+ // away messages."
+ // > It is clearly stated that publisher queues throw
+ // away old messages first, and subscriber queues
+ // are likely the same.
+ // > As we are only interested in latest measurement
+ // a short queue is used.
+ if (yaml_shouldUse_mocapMeasurements)
+ {
+ viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 3, viconCallback);
+ }
+ // LOCALISATION DATA FROM ONBOARD THE FLYING VEHICLE
+ // > The second argument is the queue_size, and the
+ // documentation states that:
+ // "If messages are arriving too fast and you are
+ // unable to keep up, roscpp will start throwing
+ // away messages."
+ // > It is clearly stated that publisher queues throw
+ // away old messages first, and subscriber queues
+ // are likely the same.
+ // > As we are only interested in latest measurement
+ // a short queue is used.
+ if (yaml_shouldUse_onboardEstimate)
+ {
+ onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 3, onboardEstimateCallback);
+ }
+
// PUBLISHER FOR COMMANDING THE CRAZYFLIE
- // i.e., motorCmd{1,2,3,4}, roll, pitch, yaw, and onboardControllerType
+ // i.e., motorCmd{1,2,3,4}, and the {x,y,z,yaw} controller modes and setpoints
//ros::Publishers to advertise the control output
m_commandForSendingToCrazyfliePublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
@@ -1660,9 +1844,7 @@ int main(int argc, char* argv[])
// SUBSCRIBER FOR CRAZY RADIO STATUS CHANGES
- // crazyradio status. Connected, connecting or disconnected
- std::string namespace_to_crazy_radio = m_namespace + "/CrazyRadio";
- ros::NodeHandle nodeHandle_to_crazy_radio(namespace_to_crazy_radio);
+ // Crazyradio status: { connected , connecting , disconnected }
ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazy_radio.subscribe("CrazyRadioStatus", 1, crazyRadioStatusCallback);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
index f4dfe8e42d2f63d9f50564f0fdc6e775da885587..54fa2eee667c074f288f84d2992baf5f2525403f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
@@ -58,46 +58,51 @@
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();
+ unsigned int quadrotor_count = m_flying_fleet_states.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)
+ for(std::vector<FlyingVehicleState>::iterator it = m_flying_fleet_states.begin(); it != m_flying_fleet_states.end(); ++it)
{
// Access that current element as *it
// Get the index if needed
- //int idx = std::distance( m_quadrotor_fleet.begin() , it );
+ int idx = std::distance( m_flying_fleet_states.begin() , it );
- // Simulate the quadrotor for one time step
- it->simulate_for_one_time_step( yaml_mocap_deltaT_in_seconds );
+ // Convert the state to global
+ float local_x = it->x;
+ float local_y = it->y;
+ float local_z = it->z;
+ float global_x = local_x + (m_flying_fleet_areaBounds[idx].xmin + m_flying_fleet_areaBounds[idx].xmax) / 2.0;
+ float global_y = local_y + (m_flying_fleet_areaBounds[idx].ymin + m_flying_fleet_areaBounds[idx].ymax) / 2.0;
+ float global_z = local_z; // + (m_flying_fleet_areaBounds[idx].zmin + m_flying_fleet_areaBounds[idx].zmax) / 2.0;
// Local variable for the data of this quadrotor
- CrazyflieData quadrotor_data;
+ FlyingVehicleState quadrotor_data;
// Fill in the details
+ // > For the type
+ quadrotor_data.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
// > For the name
- quadrotor_data.crazyflieName = it->m_id_string;
+ quadrotor_data.vehicleName = it->vehicleName;
// > For the occulsion flag
- quadrotor_data.occluded = false;
+ quadrotor_data.isValid = it->isValid;
// > For position
- quadrotor_data.x = it->m_position[0];
- quadrotor_data.y = it->m_position[1];
- quadrotor_data.z = it->m_position[2];
+ quadrotor_data.x = global_x;
+ quadrotor_data.y = global_y;
+ quadrotor_data.z = global_z;
// > 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];
+ quadrotor_data.roll = it->roll;
+ quadrotor_data.pitch = it->pitch;
+ quadrotor_data.yaw = it->yaw;
// > For the acquiring time
quadrotor_data.acquiringTime = 0.0;
@@ -114,6 +119,21 @@ void timerCallback_mocap_publisher(const ros::TimerEvent&)
+// CALLBACK FOR RECEIVEING CRAZYFLIE SIMULATION DATA
+// > This data is published by the CrazyRadioEmulator node
+void cfSimulationStateCallback(const FlyingVehicleState & msg)
+{
+ // Extract the id string
+ std::string id_as_string = msg.vehicleName;
+ // Convert the id to an integer
+ int id_as_int = std::stoi( id_as_string.substr( id_as_string.length()-2 ) );
+ // Update the corresponding entry of the "m_flying_fleet_states"
+ m_flying_fleet_states[id_as_int] = msg;
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
// L O O A A D D
@@ -145,21 +165,21 @@ 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)
+ for(std::vector<FlyingVehicleState>::iterator it = m_flying_fleet_states.begin(); it != m_flying_fleet_states.end(); ++it)
{
// Access that current element as *it
// Get the index if needed
- //int idx = std::distance( m_quadrotor_fleet.begin() , it );
+ int idx = std::distance( m_flying_fleet_states.begin() , it );
// Local variable for the service call
CMQueryCrazyflieName contextCall;
// Set the name of this quadrotor
- contextCall.request.crazyflieName = it->m_id_string;
+ contextCall.request.crazyflieName = it->vehicleName;
// Wait for the service to exist
- m_centralManagerService.waitForExistence(ros::Duration(0.5));
+ m_centralManagerService.waitForExistence(ros::Duration(0));
// Local variable for the agent ID
int new_agent_id = -1;
@@ -176,28 +196,28 @@ void loadContextForEachQuadrotor()
// 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;
+ m_flying_fleet_areaBounds[idx] = 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);
+ //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.");
+ //ROS_INFO_STREAM("[MOCAP EMULATOR] Quadrotor simulator with ID \"" << it->get_id_string() << "\" connected to agent ID " << new_agent_id << " in database.");
}
else
{
- // Let the user know that the "m_id_string" was not found
+ // Let the user know that the "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.");
+ //ROS_INFO_STREAM("[MOCAP EMULATOR] Quadrotor simulator with ID \"" << it->get_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 );
+ //it->update_commanding_agent_id( new_agent_id );
}
}
@@ -220,35 +240,35 @@ void loadContextForEachQuadrotor()
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("~");
+ // 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);
+ getParameterFloatVectorKnownLength(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);
+
+ // 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;
+ // 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);
+ // 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);
}
@@ -291,42 +311,57 @@ int main(int argc, char* argv[])
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle nodeHandle_dfall_root("/dfall");
+
+
+
// ADD QUADROTORS TO THE FLEET
- for ( int i_quad=1 ; i_quad<3 ; i_quad++ )
+ for ( int i_quad=1 ; i_quad<4 ; 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
+ // Create a string for the ID, zero padded
+ // > of length 2
+ std::ostringstream str_stream_02;
+ str_stream_02 << std::setw(2) << std::setfill('0') << i_quad;
+ std::string this_quad_id_as_string(str_stream_02.str());
+ // > of length 3
+ std::ostringstream str_stream_03;
+ str_stream_03 << std::setw(3) << std::setfill('0') << i_quad;
+ std::string this_agent_id_as_string(str_stream_03.str());
+
+ // Prepare a reset state of this flying vehicle
+ FlyingVehicleState this_state;
+ this_state.x = 0.0f;
+ this_state.y = 0.0f;
+ this_state.z = 0.0f;
+ this_state.roll = 0.0f;
+ this_state.pitch = 0.0f;
+ this_state.yaw = 0.0f;
+ this_state.isValid = false;
+ this_state.vehicleName = "CF"+this_quad_id_as_string;
+
+ // Push back into the vector of states
+ m_flying_fleet_states.push_back(this_state);
+
+ // Subscribe to the messages for this vehicle
+ m_flying_fleet_state_subscribers.push_back( nodeHandle_dfall_root.subscribe("agent"+this_agent_id_as_string+"/CrazyRadio/CFSimulationState", 50, cfSimulationStateCallback) );
+
+ // Set the Area Bounds
+ AreaBounds this_area;
+ this_area.xmin = -1.0f + this_reset_x;
+ this_area.xmax = 1.0f + this_reset_x;
+ this_area.ymin = -1.0f;
+ this_area.ymax = 1.0f;
+ this_area.zmin = 0.0f;
+ this_area.zmax = 2.0f;
+ m_flying_fleet_areaBounds.push_back( this_area );
+
+ ROS_INFO_STREAM("this_reset_x = " << this_reset_x);
- // 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();
}
@@ -346,10 +381,6 @@ int main(int argc, char* argv[])
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:
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
index 9a0545e31edbb227f31deb314f7443a02fe59a6e..675e5a323b9a50bde9683f2c4f8f7973787eabe4 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MpcControllerService.cpp
@@ -174,20 +174,6 @@ std::string namespace_to_coordinator_parameter_service;
int my_agentID = 0;
-// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
-// The "CrazyflieData" type used for the "request" variable is a
-// structure as defined in the file "CrazyflieData.msg" which has the following
-// properties:
-// string crazyflieName The name given to the Crazyflie in the Vicon software
-// float64 x The x position of the Crazyflie [metres]
-// float64 y The y position of the Crazyflie [metres]
-// float64 z The z position of the Crazyflie [metres]
-// float64 roll The roll component of the intrinsic Euler angles [radians]
-// float64 pitch The pitch component of the intrinsic Euler angles [radians]
-// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
-
@@ -223,7 +209,7 @@ void setpointCallback(const Setpoint& newSetpoint);
// LOAD PARAMETERS
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+void getParameterFloatVectorKnownLength(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);
@@ -282,22 +268,22 @@ void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&st
// The arument "request" is a structure provided to this service with the following two
// properties:
// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+// This property is itself a structure of type "FlyingVehicleState", which is defined in the
+// file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
+// This property is an array of "FlyingVehicleState" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
@@ -718,26 +704,26 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
- getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_MpcController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// Call the function that computes details an values that are needed from these
@@ -780,7 +766,7 @@ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
return val;
}
// This function DOES NOT NEED TO BE edited for successful completion of the classroom exercise
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
index 269f0a55fdacbe0a1c9219a8ec2eeedbe6074e90..4180b69d1ed5d062baa0da48388b3cba2bdeacef 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ParameterService.cpp
@@ -76,84 +76,100 @@
bool requestLoadYamlFilenameCallbackServiceCallback(LoadYamlFromFilename::Request &request, LoadYamlFromFilename::Response &response)
{
- // Put the flying state into the response variable
- requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
+ // Put the flying state into the response variable
+ m_requestLoadYamlFilenamePublisher.publish(request.stringWithHeader);
- // Put success into the response
- response.data = 1;
+ // Put success into the response
+ response.data = 1;
- // Return
- return true;
+ // Return
+ return true;
}
void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_load_with_header)
{
- // LOAD THE YAML FILE
-
- // Get the yaml file name requested
- std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
- // Get the node handle to this parameter service
- ros::NodeHandle nodeHandle("~");
- // Instantiate a local variable for the command string that will be passed to the "system":
- std::string cmd;
- // Get the abolute path to "dfall_pkg":
- std::string dfall_pkg_path = ros::package::getPath("dfall_pkg");
- // Construct the system command string for (re-)loading the parameters:
- cmd = "rosparam load " + dfall_pkg_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
- // Let the user know what is about to happen
- ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << dfall_pkg_path << " The comand line string sent to the 'system' is: " << cmd );
- // Send the "load yaml" command to the system
- system(cmd.c_str());
-
-
-
- // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
-
- // Create publisher as a local variable, using the filename
- // as the name of the message
- ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1, true);
- //ros::spinOnce();
- // Create a local variable for the message
- IntWithHeader yaml_ready_msg;
- // Specify with the data the "type" of this parameter service
- switch (m_type)
- {
- case TYPE_AGENT:
- {
- yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
- break;
- }
- case TYPE_COORDINATOR:
- {
- yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
- break;
- }
- default:
- {
- // Throw an error if the type is not recognised
- ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
- // Specify to load from the agent by default
- yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
- break;
- }
- }
- // Copy across the boolean field
- yaml_ready_msg.shouldCheckForAgentID = yaml_filename_to_load_with_header.shouldCheckForAgentID;
- // Copy across the vector of IDs
- if (yaml_filename_to_load_with_header.shouldCheckForAgentID)
- {
- for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
- {
- yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
- }
- }
- // Sleep to make the publisher known to ROS (in seconds)
- ros::Duration(2.0).sleep();
- // Send the message
- yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
-
- // Inform the user that this was published
- ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+ // LOAD THE YAML FILE
+
+ // Get the yaml file name requested
+ std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
+ // Instantiate a local variable for the command string that will be passed to the "system":
+ std::string cmd;
+ // Get the abolute path to "dfall_pkg":
+ std::string dfall_pkg_path = ros::package::getPath("dfall_pkg");
+ // Construct the system command string for (re-)loading the parameters:
+ cmd = "rosparam load " + dfall_pkg_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
+ // Let the user know what is about to happen
+ ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << dfall_pkg_path << " The comand line string sent to the 'system' is: " << cmd );
+ // Send the "load yaml" command to the system
+ system(cmd.c_str());
+
+
+
+ // PREPARE A MESSAGE THAT THE YAML FILE WAS LOADED
+
+ // Create a local variable for the message
+ IntWithHeader yaml_ready_msg;
+ // Specify with the data the "type" of this parameter service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_COORDINATOR;
+ break;
+ }
+ default:
+ {
+ // Throw an error if the type is not recognised
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ // Specify to load from the agent by default
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ }
+ // Copy across the boolean field
+ yaml_ready_msg.shouldCheckForAgentID = yaml_filename_to_load_with_header.shouldCheckForAgentID;
+ // Copy across the vector of IDs
+ if (yaml_filename_to_load_with_header.shouldCheckForAgentID)
+ {
+ for ( int i_ID=0 ; i_ID<yaml_filename_to_load_with_header.agentIDs.size() ; i_ID++ )
+ {
+ yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
+ }
+ }
+
+ // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
+
+ // Check if the publisher is already in the "map"
+ if (m_yamlParametersReadyForFetchPublisherMap.count(yaml_filename_to_load) > 0)
+ {
+ // Send the message
+ m_yamlParametersReadyForFetchPublisherMap[yaml_filename_to_load].publish(yaml_ready_msg);
+ // Inform the user that this was published
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+ }
+ else
+ {
+ // Create a publisher
+ // > using the filename as the name of the message
+ // > adding it to the map of publihsers to make the
+ // next load faster
+ // Get the node handle to this parameter service
+ ros::NodeHandle nodeHandle("~");
+ m_yamlParametersReadyForFetchPublisherMap[yaml_filename_to_load] = nodeHandle.advertise<IntWithHeader>(yaml_filename_to_load, 1);
+ // Inform the user that this publisher was added
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Added " << yaml_filename_to_load << " to the publisher map." );
+ // Sleep to make the publisher known to ROS (in seconds)
+ ros::Duration(2.0).sleep();
+ // Send the message
+ m_yamlParametersReadyForFetchPublisherMap[yaml_filename_to_load].publish(yaml_ready_msg);
+ // Inform the user that this was published
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Published message that " << yaml_filename_to_load << " yaml parameters are ready." );
+ }
}
@@ -162,89 +178,89 @@ void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_lo
bool getTypeAndIDParameters()
{
- // Initialise the return variable as success
- bool return_was_successful = true;
-
- // 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 value of the "type" parameter into a local string variable
- std::string type_string;
- if(!nodeHandle.getParam("type", type_string))
- {
- // Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("[PARAMETER SERVICE] Failed to get type");
- }
-
- // Set the "m_type" class variable based on this string loaded
- if ((!type_string.compare("coordinator")))
- {
- m_type = TYPE_COORDINATOR;
- }
- else if ((!type_string.compare("agent")))
- {
- m_type = TYPE_AGENT;
- }
- else
- {
- // Set "m_type" to the value indicating that it is invlid
- m_type = TYPE_INVALID;
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
- }
-
-
- // Construct the string to the namespace of this Paramater Service
- switch (m_type)
- {
- case TYPE_AGENT:
- {
- // Get the value of the "agentID" parameter into the class variable "m_Id"
- if(!nodeHandle.getParam("agentID", m_ID))
- {
- // Throw an error if the agent ID parameter could not be obtained
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID");
- }
- else
- {
- // Inform the user about the type and ID
- ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
- }
- break;
- }
-
- // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- case TYPE_COORDINATOR:
- {
- // Get the value of the "coordID" parameter into the class variable "m_Id"
- if(!nodeHandle.getParam("coordID", m_ID))
- {
- // Throw an error if the coord ID parameter could not be obtained
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] Failed to get coordID");
- }
- else
- {
- // Inform the user about the type and ID
- ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
- }
- break;
- }
-
- default:
- {
- // Throw an error if the type is not recognised
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
- break;
- }
- }
-
- // Return
- return return_was_successful;
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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 value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
}
@@ -253,47 +269,47 @@ bool getTypeAndIDParameters()
bool constructNamespaces()
{
- // Initialise the return variable as success
- bool return_was_successful = true;
-
- // Get the namespace of this "ParameterService" node
- std::string this_node_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << this_node_namespace);
-
- // Construct the string to the namespace of this Paramater Service
- switch (m_type)
- {
- case TYPE_AGENT:
- {
- //m_base_namespace = ros::this_node::getNamespace();
- //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
- m_base_namespace = this_node_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
- break;
- }
-
- // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- case TYPE_COORDINATOR:
- {
- //m_base_namespace = ros::this_node::getNamespace();
- //m_base_namespace = "/ParameterService";
- m_base_namespace = this_node_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
- break;
- }
-
- default:
- {
- // Throw an error if the type is not recognised
- return_was_successful = false;
- ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
- break;
- }
- }
-
- // Return
- return return_was_successful;
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Get the namespace of this "ParameterService" node
+ std::string this_node_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << this_node_namespace);
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
}
@@ -310,40 +326,69 @@ bool constructNamespaces()
int main(int argc, char* argv[])
{
- // Starting the ROS-node
- ros::init(argc, argv, "ParameterService");
-
- // 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 type and ID of this parameter service
- bool isValid_type_and_ID = getTypeAndIDParameters();
-
- // Construct the namespace into which this parameter service
- // loads yaml parameters
- bool isValid_namespaces = constructNamespaces();
-
- // Stall if the TYPE and ID are not valid
- if ( !( isValid_type_and_ID && isValid_namespaces ) )
- {
- ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
- ros::spin();
- }
-
- // Subscribe to the messages that request loading a yaml file
- ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 20, requestLoadYamlFilenameCallback);
-
- // Publisher for publishing "internally" to the subscriber above
- requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
-
- // Advertise the service for requests to load a yaml file
- ros::ServiceServer requestLoadYamlFilenameService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
-
- // Inform the user the this node is ready
- ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
- // Spin as a single-thread node
- ros::spin();
-
- return 0;
+ // Starting the ROS-node
+ ros::init(argc, argv, "ParameterService");
+
+ // 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 type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Construct the namespace into which this parameter service
+ // loads yaml parameters
+ bool isValid_namespaces = constructNamespaces();
+
+ // Stall if the TYPE and ID are not valid
+ if ( !( isValid_type_and_ID && isValid_namespaces ) )
+ {
+ ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+ // Fetch the YAML parameter that itself is the array of
+ // YAML file name strings
+ ros::NodeHandle nodeHandle_to_yamlFileNames(nodeHandle, "YamlFileNames");
+ if(!nodeHandle_to_yamlFileNames.getParam("filenames", yaml_filenames_provided))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get filenames");
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO_STREAM("[PARAMETER SERVICE] loaded " << yaml_filenames_provided.size() << " filenames for which publishers will be prepared.");
+ }
+
+
+ // For each for the filenames provided:
+ // > Create a publisher, using the filename as the name
+ // of the message
+ // > Add the publisher to a "map", i.e., to a dictionary
+ // > Inform the user that this publisher was added
+ for (std::string filename_str : yaml_filenames_provided)
+ {
+ m_yamlParametersReadyForFetchPublisherMap[filename_str] = nodeHandle.advertise<IntWithHeader>(filename_str, 1);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Added " << filename_str << " to the publisher map." );
+ }
+
+
+ // Subscribe to the messages that request loading a yaml file
+ ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 20, requestLoadYamlFilenameCallback);
+
+ // Publisher for publishing "internally" to the subscriber above
+ m_requestLoadYamlFilenamePublisher = nodeHandle.advertise <StringWithHeader>("requestLoadYamlFilename", 1);
+
+ // Advertise the service for requests to load a yaml file
+ ros::ServiceServer requestLoadYamlFilenameService = nodeHandle.advertiseService("requestLoadYamlFilename", requestLoadYamlFilenameCallbackServiceCallback);
+
+ // Inform the user the this node is ready
+ ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
index b30281bf67f157480077faf37d962938db4e5939..aa340b95b7329b0d8ae2e83810f57dc16e37d19e 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/PickerControllerService.cpp
@@ -291,56 +291,78 @@ void smoothSetpointChanges()
//
// The arument "request" is a structure provided to this service with the following two
// properties:
-// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+//
+// >> request.ownCrazyflie
+// This property is itself a structure of type "FlyingVehicleState", which is defined in the
+// file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
-// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
+// >> request.otherCrazyflies
+// This property is an array of "FlyingVehicleState" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
-// The argument "response" is a structure that is expected to be filled in by this
-// service by this function, it has only the following property
-// response.ControlCommand
-// This property is iteself a structure of type "ControlCommand", which is defined in
-// the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+// The argument "response" is a structure that is expected to be filled in by
+// this service by this function, it has only the following property
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines" section at the
-// top of this file, they are:
-// CF_COMMAND_TYPE_MOTOR
-// CF_COMMAND_TYPE_RATE
-// CF_COMMAND_TYPE_ANGLE.
-// > With CF_COMMAND_TYPE_RATE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
-// specify the angular rate in [radians/second] that will be requested from the
-// PID controllers running in the Crazyflie 2.0 firmware.
-// > With CF_COMMAND_TYPE_RATE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
-// are the baseline motor commands requested from the Crazyflie, with the adjustment
-// for body rates being added on top of this in the firmware (i.e., as per the code
-// of the "distribute_power" function provided in exercise sheet 2).
-// > With CF_COMMAND_TYPE_RATE the axis convention for the roll, pitch, and yaw body rates returned
-// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
-// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
-// thrust). To assist, teh following is an ASCII art of this convention:
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
+//
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
+//
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -650,15 +672,23 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
thrustAdjustment = thrustAdjustment / 4.0;
// > Compute the feed-forward force
float feed_forward_thrust_per_motor = m_weight_total_in_newtons / 4.0;
@@ -668,11 +698,6 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// An alternate debugging technique is to print out data directly to the
@@ -691,9 +716,9 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+ ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
@@ -1228,7 +1253,7 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
yaml_shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
@@ -1247,10 +1272,10 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// 16-BIT COMMAND LIMITS
yaml_cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
@@ -1258,13 +1283,13 @@ void fetchPickerControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", yaml_PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", yaml_PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , yaml_PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", yaml_PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", yaml_PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , yaml_PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", yaml_PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", yaml_PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , yaml_PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
index e1e01f283eeb80461d021d89b2f19b111cb771af..6ca55d901e94bd7d8dc6e9458fcb832f632f0d02 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/RemoteControllerService.cpp
@@ -79,54 +79,78 @@
//
// The arument "request" is a structure provided to this service with the following two
// properties:
-// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+
+// >> request.ownCrazyflie
+// This property is itself a structure of type "FlyingVehicleState", which is defined in the
+// file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
-// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
+// >> request.otherCrazyflies
+// This property is an array of "FlyingVehicleState" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
-// response.ControlCommand
-// This property is iteself a structure of type "ControlCommand", which is defined in
-// the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines" section at the
-// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
-// > In the classroom exercise we will only use the RATE_MODE.
-// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
-// specify the angular rate in [radians/second] that will be requested from the
-// PID controllers running in the Crazyflie 2.0 firmware.
-// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
-// are the baseline motor commands requested from the Crazyflie, with the adjustment
-// for body rates being added on top of this in the firmware (i.e., as per the code
-// of the "distribute_power" function provided in exercise sheet 2).
-// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
-// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
-// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
-// thrust). To assist, teh following is an ASCII art of this convention:
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
+//
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
+//
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -293,15 +317,23 @@ void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
float thrustAdjustment = thrustAdjustment_total / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -310,19 +342,15 @@ void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
// Display some details (if requested)
if (shouldDisplayDebugInfo)
{
ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
- ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
- ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
- ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.yControllerSetpoint );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.xControllerSetpoint );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yawControllerSetpoint );
}
}
@@ -363,11 +391,11 @@ void viconSubscribeObjectNameCallback(const ViconSubscribeObjectName& msg)
//is called when new data from Vicon arrives
void viconCallback(const ViconData& viconData)
{
- for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+ for(std::vector<FlyingVehicleState>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
{
- CrazyflieData objectData_in_globalFrame = *it;
+ FlyingVehicleState objectData_in_globalFrame = *it;
- if(objectData_in_globalFrame.crazyflieName == viconObjectName_forRemote)
+ if(objectData_in_globalFrame.vehicleName == viconObjectName_forRemote)
{
if (shouldPublishRemote_xyz_rpy)
@@ -382,8 +410,8 @@ void viconCallback(const ViconData& viconData)
ROS_INFO_STREAM("[REMOTE CONTROLLER] Remote [z,r,p,y] = [ " << objectData_in_globalFrame.z << " , " << objectData_in_globalFrame.roll << " , " << objectData_in_globalFrame.pitch << " , " << objectData_in_globalFrame.yaw << " ]" );
}
- // Update the remote set-point is not occluded
- if(!objectData_in_globalFrame.occluded)
+ // Update the remote set-point if the data isValid
+ if(objectData_in_globalFrame.isValid)
{
// Update the z height variable that is used when activating
@@ -417,7 +445,7 @@ void viconCallback(const ViconData& viconData)
}
// Publish the updated setpoint
- CrazyflieData setpointToPublish;
+ FlyingVehicleState setpointToPublish;
setpointToPublish.roll = setpointFromRemote_roll;
setpointToPublish.pitch = setpointFromRemote_pitch;
setpointToPublish.yaw = setpointFromRemote_yaw;
@@ -688,14 +716,21 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
// Display that the "controller_mode" was not recognised
ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'controller_mode' is not recognised.");
// Set everything in the response to zero
- response.controlOutput.roll = 0;
- response.controlOutput.pitch = 0;
- response.controlOutput.yaw = 0;
- response.controlOutput.motorCmd1 = 0;
- response.controlOutput.motorCmd2 = 0;
- response.controlOutput.motorCmd3 = 0;
- response.controlOutput.motorCmd4 = 0;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
+ // > Specify that all controllers are disabled
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ // > Fill in zero for the controller setpoints
+ response.controlOutput.xControllerSetpoint = 0.0;
+ response.controlOutput.yControllerSetpoint = 0.0;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = 0.0;
+ // > Fill in all motor thrusts as zerp
+ response.controlOutput.motorCmd1 = 0.0;
+ response.controlOutput.motorCmd2 = 0.0;
+ response.controlOutput.motorCmd3 = 0.0;
+ response.controlOutput.motorCmd4 = 0.0;
break;
}
}
@@ -735,15 +770,23 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
thrustAdjustment = thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -752,11 +795,6 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// An alternate debugging technique is to print out data directly to the
@@ -775,9 +813,9 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+ ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the "thrust-to-command" conversion parameters.
ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
@@ -821,15 +859,23 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGLE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGLE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGLE;
+
+ // Put the computed angles into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollAngle_forResponse;
- response.controlOutput.pitch = pitchAngle_forResponse;
- response.controlOutput.yaw = setpoint[3];
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchAngle_forResponse;
+ response.controlOutput.yControllerSetpoint = rollAngle_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = setpoint[3];
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
thrustAdjustment = thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -838,13 +884,6 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
-
-
}
@@ -926,15 +965,23 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -943,19 +990,15 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
// Display some details (if requested)
if (shouldDisplayDebugInfo)
{
ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
- ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
- ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
- ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.yControllerSetpoint );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.xControllerSetpoint );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yawControllerSetpoint );
}
}
@@ -1376,9 +1419,9 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
remoteConrtolSetpointFactor_z = getParameterFloat(nodeHandle_for_paramaters , "remoteConrtolSetpointFactor_z");
// LQR Gain matrix for tracking the angle commands from the Crazyflie
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
@@ -1403,7 +1446,7 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
@@ -1424,24 +1467,24 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// 16-BIT COMMAND LIMITS
@@ -1451,13 +1494,13 @@ void fetchRemoteControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
@@ -1635,13 +1678,13 @@ int main(int argc, char* argv[]) {
ros::Subscriber viconSubscribeObjectNameSubscriber = nodeHandle.subscribe("ViconSubscribeObjectName", 1, viconSubscribeObjectNameCallback);
// Advertise the message topic of the Remote (y,roll,pitch,yaw)
- remote_xyz_rpy_publisher = nodeHandle.advertise<CrazyflieData>("RemoteData", 1);
+ remote_xyz_rpy_publisher = nodeHandle.advertise<FlyingVehicleState>("RemoteData", 1);
// Subscribe to the message that triggers activates/deactivates remote control mode
ros::Subscriber shouldActivateSubscriber = nodeHandle.subscribe("Activate", 1, shouldActivateCallback);
// Advertise the message topic of the Remote (y,roll,pitch,yaw)
- remote_control_setpoint_publisher = nodeHandle.advertise<CrazyflieData>("RemoteControlSetpoint", 1);
+ remote_control_setpoint_publisher = nodeHandle.advertise<FlyingVehicleState>("RemoteControlSetpoint", 1);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
index e9f1c08cfd6265c2585b4c14244354f0069a6178..ea4aa64b401207346d478de2f9db40c37d5a9a99 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/SafeControllerService.cpp
@@ -289,24 +289,24 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
// > The row of the LQR matrix that commands body frame roll rate
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
// > The row of the LQR matrix that commands body frame pitch rate
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
// > The row of the LQR matrix that commands body frame yaw rate
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
// > The row of the LQR matrix that commands thrust adjustment
- getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
// > The gains for the point-mass Kalman filter
- getParameterFloatVector(nodeHandle_for_safeController, "filterGain", filterGain, 6);
- getParameterFloatVector(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "filterGain", filterGain, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
// > The defailt setpoint of the controller
- getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
// DEBUGGING: Print out one of the parameters that was loaded
ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << cf_mass);
@@ -346,7 +346,7 @@ float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
return val;
}
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+void getParameterFloatVectorKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'");
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 4266a77b9b69f81534e15062e2555bf9d534f4b2..20c0131b793834925b081da783f1649bef283811 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -37,7 +37,27 @@
#include "nodes/StudentControllerService.h"
+// ---------------------------------------------------------------------------------------------------
+// CCCC L A SSSS SSSS V V A RRRR III A BBBB L EEEEE SSSS
+// C L A A S S V V A A R R I A A B B L E S
+// C L A A SSS SSS V V A A RRRR I A A BBBB L EEE SSS
+// C L AAAAA S S V V AAAAA R R I AAAAA B B L E S
+// CCCC LLLLL A A SSSS SSSS V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ---------------------------------------------------------------------------------------------------
+//DECLARE YOUR CLASS VARIABLES BELOW HERE
+
+
+
+// VARIABLES FOR VALUES LOADED FROM THE YAML FILE
+// > the proportional gain for z control
+float yaml_kp_z = 1.0;
+
+// > the derivative gain for z control
+float yaml_kd_z = 1.0;
+
+// > the integral gain for z control
+float yaml_ki_z = 1.0;
@@ -80,64 +100,78 @@
//
// The arument "request" is a structure provided to this service with the following two
// properties:
-// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+//
+// >> request.ownCrazyflie
+// This property is itself a structure of type "FlyingVehicleState", which is defined in the
+// file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
-// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
+// >> request.otherCrazyflies
+// This property is an array of "FlyingVehicleState" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
-// response.ControlCommand
-// This property is iteself a structure of type "ControlCommand", which is defined in
-// the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines"
-// section in the header file, they are:
-// - CF_COMMAND_TYPE_MOTORS
-// - CF_COMMAND_TYPE_RATE
-// - CF_COMMAND_TYPE_ANGLE
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
//
-// > For completeing the class exercises it is only required to use
-// the CF_COMMAND_TYPE_RATE option.
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
//
-// > For the CF_COMMAND_TYPE_RATE optoin:
-// 1) the ".roll", ".ptich", and ".yaw" properties of
-// "response.ControlCommand" specify the angular rate in
-// [radians/second] that will be requested from the PID controllers
-// running in the Crazyflie 2.0 firmware.
-// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
// "response.ControlCommand" are the baseline motor commands
// requested from the Crazyflie, with the adjustment for body rates
// being added on top of this in the firmware (i.e., as per the
// code of the "distribute_power" found in the firmware).
-// 3) the axis convention for the roll, pitch, and yaw body rates
-// returned in "response.ControlCommand" should use right-hand
-// coordinate axes with x-forward and z-upwards (i.e., the positive
-// z-axis is aligned with the direction of positive thrust). To
-// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -188,20 +222,39 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
// Compute an estimate of the velocity
- // > But only if this is NOT the first call to the controller
- if (!request.isFirstControllerCall)
+ // > Via finite differences if receiveing
+ // Motion Capture data
+ if (request.ownCrazyflie.type == FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT)
{
- // > Compute as simply the derivative between the current and previous position
- stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
- stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
- stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+ // > But only if this is NOT the first call
+ // to the controller
+ if (!request.isFirstControllerCall)
+ {
+ // > Compute as simply the derivative between
+ // the current and previous position
+ stateErrorInertial[3] = (stateErrorInertial[0] - m_previous_stateErrorInertial[0]) * yaml_control_frequency;
+ stateErrorInertial[4] = (stateErrorInertial[1] - m_previous_stateErrorInertial[1]) * yaml_control_frequency;
+ stateErrorInertial[5] = (stateErrorInertial[2] - m_previous_stateErrorInertial[2]) * yaml_control_frequency;
+ }
+ else
+ {
+ // Set the velocities to zero
+ stateErrorInertial[3] = 0.0;
+ stateErrorInertial[4] = 0.0;
+ stateErrorInertial[5] = 0.0;
+ }
+ }
+ // > Else, via finite difference if receiveing
+ // onboard state estimate data
+ else if (request.ownCrazyflie.type == FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE)
+ {
+ stateErrorInertial[3] = request.ownCrazyflie.vx;
+ stateErrorInertial[4] = request.ownCrazyflie.vy;
+ stateErrorInertial[5] = request.ownCrazyflie.vz;
}
else
{
- // Set the velocities to zero
- stateErrorInertial[3] = 0.0;
- stateErrorInertial[4] = 0.0;
- stateErrorInertial[5] = 0.0;
+ ROS_ERROR_STREAM("[STUDENT CONTROLLER] Received a request.ownCrazyflie with unrecognised type, request.ownCrazyflie.type = " << request.ownCrazyflie.type );
}
// Fill in the roll and pitch angle measurements directly
@@ -260,8 +313,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// Put the computed yaw rate into the "response" variable
- response.controlOutput.yaw = yawRate_forResponse;
-
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
@@ -284,8 +339,9 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
}
- // Put the computed thrust adjustment into the "response" variable,
- // as well as adding the feed-forward thrust to counter-act gravity.
+ // Put the computed thrust adjustment into the "response" variable.
+ // > On top of the thrust adjustment, we must add the feed-forward thrust
+ // to counter-act gravity.
// > NOTE: remember that the thrust is commanded per motor, so you sohuld
// consider whether the "thrustAdjustment" computed by your
// controller needed to be divided by 4 or not.
@@ -293,12 +349,18 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
- // > NOTE: the function "computeMotorPolyBackward" converts the input argument
+ // > NOTE: the function "newtons2cmd_for_crazyflie" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd1 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd2 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd3 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+ response.controlOutput.motorCmd4 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
+
+ // Set the onboard z-controller to be OFF
+ // > This is because we commands the motor thrusts and
+ // hence an "inner" control loop is NOT needed onboard
+ // to control the z-height
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
@@ -322,7 +384,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// Put the computed pitch rate into the "response" variable
- response.controlOutput.pitch = pitchRate_forResponse;
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
@@ -347,17 +412,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
// Put the computed roll rate into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
-
-
-
- // PREPARE AND RETURN THE VARIABLE "response"
-
- // Choose the controller type use on-board the Crazyflie,
- // it can be either be Motor, Rate, or Angle based
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
@@ -394,7 +452,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
// type "float64" that you can fill in with data you would like to plot in
// real-time.
- // debugMsg.value_1 = thrustAdjustment;
+ debugMsg.value_1 = thrustAdjustment;
+ debugMsg.value_2 = rollRate_forResponse;
+ debugMsg.value_3 = pitchRate_forResponse;
+ debugMsg.value_4 = yawRate_forResponse;
// ......................
// debugMsg.value_10 = your_variable_name;
@@ -409,21 +470,16 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
// ROS_INFO_STREAM("y-coordinates: " << request.ownCrazyflie.y);
// ROS_INFO_STREAM("z-coordinates: " << request.ownCrazyflie.z);
- // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
+ // ROS_INFO_STREAM("roll: " << request.ownCrazyflie.roll);
// ROS_INFO_STREAM("pitch: " << request.ownCrazyflie.pitch);
- // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
+ // ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
// ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
// An example of "printing out" the control actions computed.
// ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- // ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- // ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- // ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
-
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- // ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
- // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
- // ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
+ // ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ // ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ // ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the per motor 16-bit command computed.
// ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
@@ -498,46 +554,6 @@ void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float y
-// ------------------------------------------------------------------------------
-// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
-// NN N E W W T O O NN N C MM MM D D
-// N N N EEE W W T O O N N N --> C M M M D D
-// N NN E W W W T O O N NN C M M D D
-// N N EEEEE W W T OOO N N CCCC M M DDDD
-//
-// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
-// C O O NN N V V E R R S I O O NN N
-// C O O N N N V V EEE RRRR SSS I O O N N N
-// C O O N NN V V E R R S I O O N NN
-// CCCC OOO N N V EEEEE R R SSSS III OOO N N
-// ----------------------------------------------------------------------------------
-
-// This function DOES NEED TO BE edited for successful completion of
-// the exercise
-float computeMotorPolyBackward(float thrust)
-{
- // Compute the 16-bit command that would produce the requested
- // "thrust" based on the quadratic mapping that is described
- // by the coefficients in the "yaml_motorPoly" variable.
- float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
-
-
-
- // > Could this formula compute a result "cmd_16bit" that is
- // greater than ((2^16)-1)?
- // > What might happen when such a number is case as a 16-bit
- // integer? In other words, what is uint16(cmd_sixteen_bit)?
-
-
-
- // Return the result
- return cmd_16bit;
-}
-
-
-
-
-
// ----------------------------------------------------------------------------------
// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
// NN N E W W S E T P P O O I NN N T
@@ -816,6 +832,15 @@ void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
// GET THE PARAMETERS:
+ // > the proportional gain for z control
+ yaml_kp_z = getParameterFloat(nodeHandle_for_paramaters , "proportionalgain_for_z");
+
+ // > the derivative gain for z control
+ yaml_kd_z = getParameterFloat(nodeHandle_for_paramaters , "derivativegain_for_z");
+
+ // > the proportional gain for z control
+ yaml_ki_z = getParameterFloat(nodeHandle_for_paramaters , "integralgain_for_z");
+
// > The mass of the crazyflie
yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
@@ -826,13 +851,9 @@ void fetchStudentControllerYamlParameters(ros::NodeHandle& nodeHandle)
// by the frequency at which the Vicon system provides position and attitude data
yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
- // > The co-efficients of the quadratic conversation from 16-bit motor command to
- // thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
-
// The default setpoint, the ordering is (x,y,z,yaw),
// with unit [meters,meters,meters,radians]
- getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
@@ -959,10 +980,6 @@ int main(int argc, char* argv[]) {
// > the mass of the crazyflie, in [grams]
yaml_cf_mass_in_grams = 25.0;
- // > the coefficients of the 16-bit command to thrust conversion
- yaml_motorPoly[0] = 5.484560e-4;
- yaml_motorPoly[1] = 1.032633e-6;
- yaml_motorPoly[2] = 2.130295e-11;
// > the frequency at which the controller is running
yaml_control_frequency = 200.0;
// > the default setpoint, the ordering is (x,y,z,yaw),
@@ -1066,6 +1083,8 @@ int main(int argc, char* argv[]) {
// And now we can instantiate the subscriber:
ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("StudentControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
+
+
// Print out some information to the user.
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
index 535d907c1d650d29a80ffa3b49beb4acc32be1f6..d92fa49846c30566517721edbeac77aacee71819 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TemplateControllerService.cpp
@@ -82,23 +82,23 @@
// following two properties:
//
// >> request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is
-// defined in the file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+// This property is itself a structure of type "FlyingVehicleState", which is
+// defined in the file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the
// motion capture system of the Crazyflie that is to be controlled by this
// service.
//
// >> request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access
+// This property is an array of "FlyingVehicleState" structures, what allows access
// to the motion capture measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by
@@ -107,39 +107,52 @@
// >> response.ControlCommand
// This property is iteself a structure of type "ControlCommand", which is
// defined in the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines"
-// section in the header file, they are:
-// - CF_COMMAND_TYPE_MOTORS
-// - CF_COMMAND_TYPE_RATE
-// - CF_COMMAND_TYPE_ANGLE
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
//
-// > For most common option to use is CF_COMMAND_TYPE_RATE option.
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
//
-// > For the CF_COMMAND_TYPE_RATE optoin:
-// 1) the ".roll", ".ptich", and ".yaw" properties of
-// "response.ControlCommand" specify the angular rate in
-// [radians/second] that will be requested from the PID controllers
-// running in the Crazyflie 2.0 firmware.
-// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
// "response.ControlCommand" are the baseline motor commands
// requested from the Crazyflie, with the adjustment for body rates
// being added on top of this in the firmware (i.e., as per the
// code of the "distribute_power" found in the firmware).
-// 3) the axis convention for the roll, pitch, and yaw body rates
-// returned in "response.ControlCommand" should use right-hand
-// coordinate axes with x-forward and z-upwards (i.e., the positive
-// z-axis is aligned with the direction of positive thrust). To
-// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -263,17 +276,23 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
}
- // Put the computed body rate commands into the "response" variable
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
-
- // Put the thrust commands into the "response" variable.
- // The thrust adjustment computed by the controller need to be added to the
- // the feed-forward thrust that "counter-acts" gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
// > NOTE: the "m_cf_weight_in_newtons" value is the total thrust required
// as feed-forward. Assuming the the Crazyflie is symmetric, this
// value is divided by four.
@@ -286,16 +305,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrust_request_per_motor);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrust_request_per_motor);
-
- // PREPARE AND RETURN THE VARIABLE "response"
-
- // Choose the controller type use on-board the Crazyflie,
- // it can be either be Motor, Rate, or Angle based
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
-
-
if (yaml_shouldPublishDebugMessage)
@@ -363,20 +372,15 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+ ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the per motor 16-bit command computed.
ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
-
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- // ROS_INFO_STREAM("motorPoly 0:" << yaml_motorPoly[0]);
- // ROS_INFO_STREAM("motorPoly 1:" << yaml_motorPoly[1]);
- // ROS_INFO_STREAM("motorPoly 2:" << yaml_motorPoly[2]);
}
// Return "true" to indicate that the control computation was
@@ -765,7 +769,7 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor
// command to thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
// > The min and max for saturating 16 bit thrust commands
yaml_command_sixteenbit_min = getParameterFloat(nodeHandle_for_paramaters, "command_sixteenbit_min");
@@ -773,7 +777,7 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// The default setpoint, the ordering is (x,y,z,yaw),
// with unit [meters,meters,meters,radians]
- getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
// Boolean indiciating whether the "Debug Message" of this agent
// should be published or not
@@ -785,10 +789,10 @@ void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
// The LQR Controller parameters
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
// > DEBUGGING: Print out one of the parameters that was loaded to
// debug if the fetching of parameters worked correctly
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp
index 9dafb62fcc64fa75f58917cc8ef22c9c25556519..92c5b4a5bf42bdee7bd05955b995cdc3d0620c05 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TestMotorsControllerService.cpp
@@ -73,7 +73,7 @@
// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
// ----------------------------------------------------------------------------------
-// This function is the callback that is linked to the "TemplateController"
+// This function is the callback that is linked to the "TestMotorsController"
// service that is advertised in the main function. This must have arguments
// that match the "input-output" behaviour defined in the "Controller.srv"
// file (located in the "srv" folder)
@@ -82,23 +82,23 @@
// following two properties:
//
// >> request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is
-// defined in the file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+// This property is itself a structure of type "FlyingVehicleState", which is
+// defined in the file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the
// motion capture system of the Crazyflie that is to be controlled by this
// service.
//
// >> request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access
+// This property is an array of "FlyingVehicleState" structures, what allows access
// to the motion capture measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by
@@ -107,39 +107,52 @@
// >> response.ControlCommand
// This property is iteself a structure of type "ControlCommand", which is
// defined in the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines"
-// section in the header file, they are:
-// - CF_COMMAND_TYPE_MOTORS
-// - CF_COMMAND_TYPE_RATE
-// - CF_COMMAND_TYPE_ANGLE
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
//
-// > For most common option to use is CF_COMMAND_TYPE_RATE option.
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
//
-// > For the CF_COMMAND_TYPE_RATE optoin:
-// 1) the ".roll", ".ptich", and ".yaw" properties of
-// "response.ControlCommand" specify the angular rate in
-// [radians/second] that will be requested from the PID controllers
-// running in the Crazyflie 2.0 firmware.
-// 2) the ".motorCmd1" to ".motorCmd4" properties of
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
// "response.ControlCommand" are the baseline motor commands
// requested from the Crazyflie, with the adjustment for body rates
// being added on top of this in the firmware (i.e., as per the
// code of the "distribute_power" found in the firmware).
-// 3) the axis convention for the roll, pitch, and yaw body rates
-// returned in "response.ControlCommand" should use right-hand
-// coordinate axes with x-forward and z-upwards (i.e., the positive
-// z-axis is aligned with the direction of positive thrust). To
-// assist, the following is an ASCII art of this convention.
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -181,16 +194,25 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// computation here, or you may find it convienient to create functions
// to keep you code cleaner
+ // Initialise a static variable for counting time "ticks"
static int ticks = 0;
-
+ // Increment the time counter every time this function is called
ticks++;
+ // Specify that all controllers are disabled
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
- response.controlOutput.roll = 0.0f;
- response.controlOutput.pitch = 0.0f;
- response.controlOutput.yaw = 0.0f;
+ // Fill in zero for the controller setpoints
+ response.controlOutput.xControllerSetpoint = 0.0;
+ response.controlOutput.yControllerSetpoint = 0.0;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = 0.0;
+ // Fill in all motor thrusts as zero
response.controlOutput.motorCmd1 = 0;
response.controlOutput.motorCmd2 = 0;
response.controlOutput.motorCmd3 = 0;
@@ -212,14 +234,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
- // PREPARE AND RETURN THE VARIABLE "response"
-
- // Choose the controller type use on-board the Crazyflie,
- // it can be either be Motor, Rate, or Angle based
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
-
// Return "true" to indicate that the control computation was
// performed successfully
@@ -229,396 +243,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
-// ------------------------------------------------------------------------------
-// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
-// R R O O T A A T E I NN N T O O
-// RRRR O O T A A T EEE I N N N T O O
-// R R O O T AAAAA T E I N NN T O O
-// R R OOO T A A T EEEEE III N N T OOO
-//
-// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
-// B B O O D D Y Y F R R A A MM MM E
-// BBBB O O D D Y FFF RRRR A A M M M EEE
-// B B O O D D Y F R R AAAAA M M E
-// BBBB OOO DDDD Y F R R A A M M EEEEE
-// ----------------------------------------------------------------------------------
-
-// The arguments for this function are as follows:
-// stateInertial
-// This is an array of length 9 with the estimates the error of of the following values
-// relative to the sepcifed setpoint:
-// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
-// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
-// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
-// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
-// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
-//
-// stateBody
-// This is an empty array of length 9, this function should fill in all elements of this
-// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
-// position and (x,y) velocities are rotated into the body frame.
-//
-// yaw_measured
-// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
-// the Vicon motion capture system
-//
-void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured)
-{
- float sinYaw = sin(yaw_measured);
- float cosYaw = cos(yaw_measured);
-
- // Fill in the (x,y,z) position estimates to be returned
- stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
- stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
- stateBody[2] = stateInertial[2];
-
- // Fill in the (x,y,z) velocity estimates to be returned
- stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
- stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
- stateBody[5] = stateInertial[5];
-
- // Fill in the (roll,pitch,yaw) estimates to be returned
- stateBody[6] = stateInertial[6];
- stateBody[7] = stateInertial[7];
- stateBody[8] = stateInertial[8];
-}
-
-
-
-
-
-// ------------------------------------------------------------------------------
-// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
-// NN N E W W T O O NN N C MM MM D D
-// N N N EEE W W T O O N N N --> C M M M D D
-// N NN E W W W T O O N NN C M M D D
-// N N EEEEE W W T OOO N N CCCC M M DDDD
-//
-// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
-// C O O NN N V V E R R S I O O NN N
-// C O O N N N V V EEE RRRR SSS I O O N N N
-// C O O N NN V V E R R S I O O N NN
-// CCCC OOO N N V EEEEE R R SSSS III OOO N N
-// ----------------------------------------------------------------------------------
-
-float computeMotorPolyBackward(float thrust)
-{
- // Compute the 16-but command that would produce the requested
- // "thrust" based on the quadratic mapping that is described
- // by the coefficients in the "yaml_motorPoly" variable.
- float cmd_16bit = (-yaml_motorPoly[1] + sqrt(yaml_motorPoly[1] * yaml_motorPoly[1] - 4 * yaml_motorPoly[2] * (yaml_motorPoly[0] - thrust))) / (2 * yaml_motorPoly[2]);
-
- // Saturate the signal to be 0 or in the range [1000,65000]
- if (cmd_16bit < yaml_command_sixteenbit_min)
- {
- cmd_16bit = 0;
- }
- else if (cmd_16bit > yaml_command_sixteenbit_max)
- {
- cmd_16bit = yaml_command_sixteenbit_max;
- }
- // Return the result
- return cmd_16bit;
-}
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
-// NN N E W W S E T P P O O I NN N T
-// N N N EEE W W SSS EEE T PPPP O O I N N N T
-// N NN E W W W S E T P O O I N NN T
-// N N EEEEE W W SSSS EEEEE T P OOO III N N T
-//
-// CCCC A L L BBBB A CCCC K K
-// C A A L L B B A A C K K
-// C A A L L BBBB A A C KKK
-// C AAAAA L L B B AAAAA C K K
-// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
-// ----------------------------------------------------------------------------------
-
-
-// REQUEST SETPOINT CHANGE CALLBACK
-void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
-{
- // Check whether the message is relevant
- bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
-
- // Continue if the message is relevant
- if (isRevelant)
- {
- // Check if the request if for the default setpoint
- if (newSetpoint.buttonID == REQUEST_DEFAULT_SETPOINT_BUTTON_ID)
- {
- setNewSetpoint(
- yaml_default_setpoint[0],
- yaml_default_setpoint[1],
- yaml_default_setpoint[2],
- yaml_default_setpoint[3]
- );
- }
- else
- {
- // Call the function for actually setting the setpoint
- setNewSetpoint(
- newSetpoint.x,
- newSetpoint.y,
- newSetpoint.z,
- newSetpoint.yaw
- );
- }
- }
-}
-
-
-// CHANGE SETPOINT FUNCTION
-void setNewSetpoint(float x, float y, float z, float yaw)
-{
- // Put the new setpoint into the class variable
- m_setpoint[0] = x;
- m_setpoint[1] = y;
- m_setpoint[2] = z;
- m_setpoint[3] = yaw;
-
- // Publish the change so that the network is updated
- // (mainly the "flying agent GUI" is interested in
- // displaying this change to the user)
-
- // Instantiate a local variable of type "SetpointWithHeader"
- SetpointWithHeader msg;
- // Fill in the setpoint
- msg.x = x;
- msg.y = y;
- msg.z = z;
- msg.yaw = yaw;
- // Publish the message
- m_setpointChangedPublisher.publish(msg);
-}
-
-
-// GET CURRENT SETPOINT SERVICE CALLBACK
-bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response)
-{
- // Directly put the current setpoint into the response
- response.setpointWithHeader.x = m_setpoint[0];
- response.setpointWithHeader.y = m_setpoint[1];
- response.setpointWithHeader.z = m_setpoint[2];
- response.setpointWithHeader.yaw = m_setpoint[3];
- // Return
- return true;
-}
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// CCCC U U SSSS TTTTT OOO M M
-// C U U S T O O MM MM
-// C U U SSS T O O M M M
-// C U U S T O O M M
-// CCCC UUU SSSS T OOO M M
-//
-// CCCC OOO M M M M A N N DDDD
-// C O O MM MM MM MM A A NN N D D
-// C O O M M M M M M A A N N N D D
-// C O O M M M M AAAAA N NN D D
-// CCCC OOO M M M M A A N N DDDD
-// ----------------------------------------------------------------------------------
-
-// CUSTOM COMMAND RECEIVED CALLBACK
-void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived)
-{
- // Check whether the message is relevant
- bool isRevelant = checkMessageHeader( m_agentID , commandReceived.shouldCheckForAgentID , commandReceived.agentIDs );
-
- if (isRevelant)
- {
- // Extract the data from the message
- int custom_button_index = commandReceived.button_index;
- float float_data = commandReceived.float_data;
- //std::string string_data = commandReceived.string_data;
-
- // Switch between the button pressed
- switch(custom_button_index)
- {
-
- // > FOR CUSTOM BUTTON 1
- case 1:
- {
- // Let the user know that this part of the code was triggered
- ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
- // Code here to respond to custom button 1
-
- break;
- }
-
- // > FOR CUSTOM BUTTON 2
- case 2:
- {
- // Let the user know that this part of the code was triggered
- ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
- // Code here to respond to custom button 2
-
- break;
- }
-
- // > FOR CUSTOM BUTTON 3
- case 3:
- {
- // Let the user know that this part of the code was triggered
- ROS_INFO_STREAM("[TEMPLATE CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
- // Code here to respond to custom button 3
-
- break;
- }
-
- default:
- {
- // Let the user know that the command was not recognised
- ROS_INFO_STREAM("[TEMPLATE CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.float_data = " << float_data );
- break;
- }
- }
- }
-}
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// 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
-// ----------------------------------------------------------------------------------
-
-
-// CALLBACK NOTIFYING THAT THE YAML PARAMETERS ARE READY TO BE LOADED
-void isReadyTemplateControllerYamlCallback(const IntWithHeader & msg)
-{
- // Check whether the message is relevant
- bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
-
- // Continue if the message is relevant
- if (isRevelant)
- {
- // Extract the data
- int parameter_service_to_load_from = msg.data;
- // Initialise a local variable for the namespace
- std::string namespace_to_use;
- // Load from the respective parameter service
- switch(parameter_service_to_load_from)
- {
- // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case LOAD_YAML_FROM_AGENT:
- {
- ROS_INFO("[TEMPLATE CONTROLLER] Now fetching the TemplateController YAML parameter values from this agent.");
- namespace_to_use = m_namespace_to_own_agent_parameter_service;
- break;
- }
- // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case LOAD_YAML_FROM_COORDINATOR:
- {
- ROS_INFO("[TEMPLATE CONTROLLER] Now fetching the TemplateController YAML parameter values from this agent's coordinator.");
- namespace_to_use = m_namespace_to_coordinator_parameter_service;
- break;
- }
-
- default:
- {
- ROS_ERROR("[TEMPLATE CONTROLLER] Paramter service to load from was NOT recognised.");
- namespace_to_use = m_namespace_to_own_agent_parameter_service;
- break;
- }
- }
- // Create a node handle to the selected parameter service
- ros::NodeHandle nodeHandle_to_use(namespace_to_use);
- // Call the function that fetches the parameters
- fetchTemplateControllerYamlParameters(nodeHandle_to_use);
- }
-}
-
-
-// LOADING OF THE YAML PARAMTERS
-void fetchTemplateControllerYamlParameters(ros::NodeHandle& nodeHandle)
-{
- // Here we load the parameters that are specified in the file:
- // TemplateController.yaml
-
- // Add the "TemplateController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "TemplateController");
-
-
-
- // GET THE PARAMETERS:
-
- // > The mass of the crazyflie
- yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
-
- // > The frequency at which the "computeControlOutput" is being called,
- // as determined by the frequency at which the motion capture system
- // provides position and attitude data
- yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
-
- // > The co-efficients of the quadratic conversation from 16-bit motor
- // command to thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", yaml_motorPoly, 3);
-
- // > The min and max for saturating 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");
-
- // The default setpoint, the ordering is (x,y,z,yaw),
- // with unit [meters,meters,meters,radians]
- getParameterFloatVector(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
-
- // Boolean indiciating whether the "Debug Message" of this agent
- // should be published or not
- yaml_shouldPublishDebugMessage = getParameterBool(nodeHandle_for_paramaters, "shouldPublishDebugMessage");
-
- // Boolean indiciating whether the debugging ROS_INFO_STREAM should
- // be displayed or not
- yaml_shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_paramaters, "shouldDisplayDebugInfo");
-
- // The LQR Controller parameters
- // The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", yaml_gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", yaml_gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", yaml_gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", yaml_gainMatrixYawRate, 9);
-
- // > DEBUGGING: Print out one of the parameters that was loaded to
- // debug if the fetching of parameters worked correctly
- ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: the fetched TemplateController/mass = " << yaml_cf_mass_in_grams);
-
-
-
- // PROCESS THE PARAMTERS
-
- // > Compute the feed-forward force that we need to counteract
- // gravity (i.e., mg) in units of [Newtons]
- m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
-
- // DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("[TEMPLATE CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
-}
-
-
@@ -677,141 +301,10 @@ int main(int argc, char* argv[]) {
- // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
-
- // NOTES:
- // > The parameters that are specified thorugh the *.yaml files
- // are managed by a separate node called the "Parameter Service"
- // > A separate node is used for reasons of speed and generality
- // > To allow for a distirbuted architecture, there are two
- // "ParamterService" nodes that are relevant:
- // 1) the one that is nested under the "m_agentID" namespace
- // 2) the one that is nested under the "m_coordID" namespace
- // > The following lines of code create the namespace (as strings)
- // to there two relevant "ParameterService" nodes.
- // > The node handles are also created because they are needed
- // for the ROS Subscriptions that follow.
-
- // Set the class variable "m_namespace_to_own_agent_parameter_service",
- // i.e., the namespace of parameter service for this agent
- m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
-
- // Set the class variable "m_namespace_to_coordinator_parameter_service",
- // i.e., the namespace of parameter service for this agent's coordinator
- constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
-
- // Inform the user of what namespaces are being used
- ROS_INFO_STREAM("[TEST MOTORS CONTROLLER] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("[TEST MOTORS CONTROLLER] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
-
- // Create, as local variables, node handles to the parameters services
- ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
- ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
-
-
-
- // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
-
- // The parameter service publishes messages with names of the form:
- // /dfall/.../ParameterService/<filename with .yaml extension>
- ros::Subscriber safeContoller_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "TemplateController", 1, isReadyTemplateControllerYamlCallback);
- ros::Subscriber safeContoller_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("TemplateController", 1, isReadyTemplateControllerYamlCallback);
-
-
-
- // GIVE YAML VARIABLES AN INITIAL VALUE
- // This can be done either here or as part of declaring the
- // variables in the header file
-
-
-
-
- // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
-
- // The yaml files for the controllers are not added to
- // "Parameter Service" as part of launching.
- // The process for loading the yaml parameters is to send a
- // 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
- // we stall the availability of this node until the
- // paramter service is ready
-
- // 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("[TEST MOTORS CONTROLLER] The request load yaml file service call failed.");
- }
-
-
-
-
// PUBLISHERS AND SUBSCRIBERS
- // Instantiate the class variable "m_debugPublisher" to be a
- // "ros::Publisher". This variable advertises under the name
- // "DebugTopic" and is a message with the structure defined
- // in the file "DebugMsg.msg" (located in the "msg" folder).
- m_debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
-
- // Instantiate the local variable "requestSetpointChangeSubscriber"
- // to be a "ros::Subscriber" type variable that subscribes to the
- // "RequestSetpointChange" topic and calls the class function
- // "requestSetpointChangeCallback" each time a messaged is received
- // on this topic and the message is passed as an input argument to
- // the callback function. This subscriber will mainly receive
- // messages from the "flying agent GUI" when the setpoint is changed
- // by the user.
- ros::Subscriber requestSetpointChangeSubscriber = nodeHandle.subscribe("RequestSetpointChange", 1, requestSetpointChangeCallback);
-
- // Same again but instead for changes requested by the coordinator.
- // For this we need to first create a node handle to the coordinator:
- std::string namespace_to_coordinator;
- constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
- ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
- // And now we can instantiate the subscriber:
- ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TemplateControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
-
- // Instantiate the class variable "m_setpointChangedPublisher" to
- // be a "ros::Publisher". This variable advertises under the name
- // "SetpointChanged" and is a message with the structure defined
- // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
- // This publisher is used by the "flying agent GUI" to update the
- // field that displays the current setpoint for this controller.
- m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
-
- // Instantiate the local variable "getCurrentSetpointService" to be
- // a "ros::ServiceServer" type variable that advertises the service
- // called "GetCurrentSetpoint". This service has the input-output
- // behaviour defined in the "GetSetpointService.srv" file (located
- // in the "srv" folder). This service, when called, is provided with
- // an integer (that is essentially ignored), and is expected to respond
- // with the current setpoint of the controller. When a request is made
- // of this service the "getCurrentSetpointCallback" function is called.
- ros::ServiceServer getCurrentSetpointService = nodeHandle.advertiseService("GetCurrentSetpoint", getCurrentSetpointCallback);
-
-
-
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
- // variable that advertises the service called "TemplateController". This service has
+ // variable that advertises the service called "TestMotorsController". This service has
// the input-output behaviour defined in the "Controller.srv" file (located in the
// "srv" folder). This service, when called, is provided with the most recent
// measurement of the Crazyflie and is expected to respond with the control action
@@ -820,20 +313,6 @@ int main(int argc, char* argv[]) {
// of this service the "calculateControlOutput" function is called.
ros::ServiceServer service = nodeHandle.advertiseService("TestMotorsController", calculateControlOutput);
- // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
- // type variable that subscribes to the "GUIButton" topic and calls the class
- // function "customCommandReceivedCallback" each time a messaged is received on this topic
- // and the message received is passed as an input argument to the callback function.
- ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("CustomButtonPressed", 1, customCommandReceivedCallback);
-
- // Same again but instead for changes requested by the coordinator.
- // For this we need to first create a node handle to the coordinator:
- //std::string namespace_to_coordinator;
- //constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
- //ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
- // And now we can instantiate the subscriber:
- ros::Subscriber customCommandReceivedSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("TemplateControllerService/CustomButtonPressed", 1, customCommandReceivedCallback);
-
// Print out some information to the user.
@@ -844,4 +323,4 @@ int main(int argc, char* argv[]) {
// 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/TuningControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
index d18bd7ee6ad97304e497a71f9699f2536ca41c7a..ab91b275c584e51c349e61ec1ee5d02f5193490b 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TuningControllerService.cpp
@@ -79,54 +79,78 @@
//
// The arument "request" is a structure provided to this service with the following two
// properties:
-// request.ownCrazyflie
-// This property is itself a structure of type "CrazyflieData", which is defined in the
-// file "CrazyflieData.msg", and has the following properties
-// string crazyflieName
+//
+// >> request.ownCrazyflie
+// This property is itself a structure of type "FlyingVehicleState", which is defined in the
+// file "FlyingVehicleState.msg", and has the following properties
+// string vehicleName
// float64 x The x position of the Crazyflie [metres]
// float64 y The y position of the Crazyflie [metres]
// float64 z The z position of the Crazyflie [metres]
// float64 roll The roll component of the intrinsic Euler angles [radians]
// float64 pitch The pitch component of the intrinsic Euler angles [radians]
// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// float64 acquiringTime #delta t The time elapsed since the previous "FlyingVehicleState" was received [seconds]
+// bool isValid A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
-// request.otherCrazyflies
-// This property is an array of "CrazyflieData" structures, what allows access to the
+// >> request.otherCrazyflies
+// This property is an array of "FlyingVehicleState" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
-// response.ControlCommand
-// This property is iteself a structure of type "ControlCommand", which is defined in
-// the file "ControlCommand.msg", and has the following properties:
-// float32 roll The command sent to the Crazyflie for the body frame x-axis
-// float32 pitch The command sent to the Crazyflie for the body frame y-axis
-// float32 yaw The command sent to the Crazyflie for the body frame z-axis
-// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
-// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
-// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// >> response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is
+// defined in the file "ControlCommand.msg", and has the following properties:
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 ... same for motor 2
+// uint16 motorCmd3 ... same for motor 3
+// uint16 motorCmd4 ... same for motor 4
+// uint8 xControllerMode The mode sent to the Crazyflie for what controller to run for the body frame x-axis
+// uint8 yControllerMode ... same body frame y-axis
+// uint8 zControllerMode ... same body frame z-axis
+// uint8 yawControllerMode ... same body frame yaw
+// float32 xControllerSetpoint The setpoint sent to the Crazyflie for the body frame x-axis controller
+// float32 yControllerSetpoint ... same body frame y-axis
+// float32 zControllerSetpoint ... same body frame z-axis
+// float32 yawControllerSetpoint ... same body frame yaw
//
-// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
-// > The allowed values for "onboardControllerType" are in the "Defines" section at the
-// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
-// > In the classroom exercise we will only use the RATE_MODE.
-// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
-// specify the angular rate in [radians/second] that will be requested from the
-// PID controllers running in the Crazyflie 2.0 firmware.
-// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
-// are the baseline motor commands requested from the Crazyflie, with the adjustment
-// for body rates being added on top of this in the firmware (i.e., as per the code
-// of the "distribute_power" function provided in exercise sheet 2).
-// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
-// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
-// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
-// thrust). To assist, teh following is an ASCII art of this convention:
+// IMPORTANT NOTES FOR "{x,y,z,yaw}ControllerMode" AND AXIS CONVENTIONS
+// > The allowed values for "{x,y,z,yaw}ControllerMode" are in the
+// "Constants.h" header file, they are:
+// - CF_ONBOARD_CONTROLLER_MODE_OFF
+// - CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE
+// - CF_ONBOARD_CONTROLLER_MODE_ANGLE
+// - CF_ONBOARD_CONTROLLER_MODE_VELOCITY
+// - CF_ONBOARD_CONTROLLER_MODE_POSITION
+//
+// > The most common option to use for the {x,y,yaw} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE option.
+//
+// > The most common option to use for the {z} controller is
+// the CF_ONBOARD_CONTROLLER_MODE_OFF option, and thus the
+// body frame z-axis is controlled by the motorCmd{1,2,3,4}
+// values that you set.
+//
+// > When the CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE is selected, then:
+// 1) the ".xControllerSetpoint", ".yControllerSetpoint", and
+// ".yawControllerSetpoint" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested
+// from the PID controllers running in the Crazyflie 2.0 firmware.
+// 2) the axis convention for the roll, pitch, and yaw body rates,
+// i.e., as set in the {y,x,yaw}ControllerSetpoint properties of
+// the "response.ControlCommand" that you return, is a right-hand
+// coordinate axes with x-forward and z-upwards (i.e., the positive
+// z-axis is aligned with the direction of positive thrust). To
+// assist, the ASCII art below depicts this convention.
+// 3) the ".motorCmd1" to ".motorCmd4" properties of
+// "response.ControlCommand" are the baseline motor commands
+// requested from the Crazyflie, with the adjustment for body rates
+// being added on top of this in the firmware (i.e., as per the
+// code of the "distribute_power" found in the firmware).
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
@@ -757,14 +781,21 @@ void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request
// Display that the "controller_mode" was not recognised
ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'controller_mode' is not recognised.");
// Set everything in the response to zero
- response.controlOutput.roll = 0;
- response.controlOutput.pitch = 0;
- response.controlOutput.yaw = 0;
- response.controlOutput.motorCmd1 = 0;
- response.controlOutput.motorCmd2 = 0;
- response.controlOutput.motorCmd3 = 0;
- response.controlOutput.motorCmd4 = 0;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // > Specify that all controllers are disabled
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ // > Fill in zero for the controller setpoints
+ response.controlOutput.xControllerSetpoint = 0.0;
+ response.controlOutput.yControllerSetpoint = 0.0;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = 0.0;
+ // > Fill in all motor thrusts as zerp
+ response.controlOutput.motorCmd1 = 0.0;
+ response.controlOutput.motorCmd2 = 0.0;
+ response.controlOutput.motorCmd3 = 0.0;
+ response.controlOutput.motorCmd4 = 0.0;
break;
}
}
@@ -804,15 +835,23 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
thrustAdjustment = thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -821,11 +860,6 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
// An alternate debugging technique is to print out data directly to the
@@ -844,14 +878,9 @@ void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller:
// An example of "printing out" the control actions computed.
ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
- ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
- ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
- ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
-
- // An example of "printing out" the "thrust-to-command" conversion parameters.
- ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
- ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
- ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+ ROS_INFO_STREAM("controlOutput.xControllerSetpoint = " << response.controlOutput.xControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yControllerSetpoint = " << response.controlOutput.yControllerSetpoint);
+ ROS_INFO_STREAM("controlOutput.yawControllerSetpoint = " << response.controlOutput.yawControllerSetpoint);
// An example of "printing out" the per motor 16-bit command computed.
ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
@@ -890,15 +919,23 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGLE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGLE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGLE;
+
+ // Put the computed angles into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollAngle_forResponse;
- response.controlOutput.pitch = pitchAngle_forResponse;
- response.controlOutput.yaw = setpoint[3];
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchAngle_forResponse;
+ response.controlOutput.yControllerSetpoint = rollAngle_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = setpoint[3];
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
thrustAdjustment = thrustAdjustment / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -906,14 +943,6 @@ void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
-
-
}
@@ -1013,15 +1042,23 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// UPDATE THE "RETURN" THE VARIABLE NAMED "response"
- // Put the computed rates and thrust into the "response" variable
+ // Specify the mode of each controller
+ response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+
+ // Put the computed rates into the "response" variable
// > For roll, pitch, and yaw:
- response.controlOutput.roll = rollRate_forResponse;
- response.controlOutput.pitch = pitchRate_forResponse;
- response.controlOutput.yaw = yawRate_forResponse;
- // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
+ response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.zControllerSetpoint = 0.0;
+ response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
+
+ // Put the computed thrust into the "response" variable
+ // > On top of the thrust adjustment, we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, hence divide
+ // the thrusts by 4.
float thrustAdjustment = thrustAdjustment_200Hz / 4.0;
// > NOTE: the "gravity_force_quarter" value was already divided by 4 when
// it was loaded/processes from the .yaml file.
@@ -1030,19 +1067,15 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
-
- // Specify that this controller is a rate controller
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
- response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
- // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
// Display some details (if requested)
if (shouldDisplayDebugInfo)
{
ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
- ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
- ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
- ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.yControllerSetpoint );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.xControllerSetpoint );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yawControllerSetpoint );
}
}
@@ -1481,20 +1514,20 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// PARAMETERS SPECIFIC TO THE TUNING CONTROL FEATURE
/// Setpoint for the HORIZONTAL test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_horizontal_setpoint2", test_horizontal_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_horizontal_setpoint2", test_horizontal_setpoint2, 4);
// Setpoint for the VERTICAL test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_vertical_setpoint2", test_vertical_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_vertical_setpoint2", test_vertical_setpoint2, 4);
// Setpoint for the HEADING test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_heading_setpoint1", test_heading_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_heading_setpoint2", test_heading_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_heading_setpoint1", test_heading_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_heading_setpoint2", test_heading_setpoint2, 4);
// Setpoint for the ALL test
- getParameterFloatVector(nodeHandle_for_paramaters, "test_all_setpoint1", test_all_setpoint1, 4);
- getParameterFloatVector(nodeHandle_for_paramaters, "test_all_setpoint2", test_all_setpoint2, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_all_setpoint1", test_all_setpoint1, 4);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "test_all_setpoint2", test_all_setpoint2, 4);
// Parameters for flying in a circle
test_circle_radius = getParameterFloat(nodeHandle_for_paramaters, "test_circle_radius");
@@ -1536,7 +1569,7 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "motorPoly", motorPoly, 3);
// > The boolean for whether to execute the convert into body frame function
shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_paramaters, "shouldPerformConvertIntoBodyFrame");
@@ -1557,24 +1590,24 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
yaml_estimator_method = getParameterInt( nodeHandle_for_paramaters , "estimator_method" );
// The LQR Controller parameters for "LQR_MODE_RATE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate", gainMatrixYawRate, 9);
// The LQR Controller parameters for "LQR_MODE_ANGLE"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
// The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
- getParameterFloatVector(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
// 16-BIT COMMAND LIMITS
@@ -1584,13 +1617,13 @@ void fetchTuningControllerYamlParameters(ros::NodeHandle& nodeHandle)
// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
// > For the (roll,pitch,yaw) angles
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
- getParameterFloatVector(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
// DEBUGGING: Print out one of the parameters that was loaded
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
index be04f3de12f27ea6762ce2ed125fd919b982156d..9d2a762cf232b0ab6595fe28fc99d620d633402a 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
@@ -30,12 +30,21 @@
// ----------------------------------------------------------------------------------
+// Include useful libraries
#include <string.h>
-#include "DataStreamClient.h"
#include "ros/ros.h"
+
+// Include the VICON Data Steam SDK header
+#include "DataStreamClient.h"
+
+// Include the DFALL message types
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/UnlabeledMarker.h"
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+
// #define TESTING_FAKE_DATA
// notice that unit here are in milimeters
@@ -243,21 +252,29 @@ int main(int argc, char* argv[]) {
}
- //build message
- CrazyflieData cfData;
- cfData.crazyflieName = subjectName;
-
- cfData.occluded = outputTranslation.Occluded;
-
- cfData.x = outputTranslation.Translation[0] / 1000.0f;
- cfData.y = outputTranslation.Translation[1] / 1000.0f;
- cfData.z = outputTranslation.Translation[2] / 1000.0f;
- cfData.roll = roll;
- cfData.pitch = pitch;
- cfData.yaw = yaw;
- cfData.acquiringTime = totalViconLatency;
+ // Local variable for the data of this objcet
+ FlyingVehicleState object_data;
+ // Fill in the details
+ // > For the type
+ object_data.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
+ // > For the name
+ object_data.vehicleName = subjectName;
+ // > For the occulsion flag
+ object_data.isValid = !outputTranslation.Occluded;
+ // > For position
+ object_data.x = outputTranslation.Translation[0] / 1000.0f;
+ object_data.y = outputTranslation.Translation[1] / 1000.0f;
+ object_data.z = outputTranslation.Translation[2] / 1000.0f;
+ // > For euler angles
+ object_data.roll = roll;
+ object_data.pitch = pitch;
+ object_data.yaw = yaw;
+ // > For the acquiring time
+ object_data.acquiringTime = totalViconLatency;
+
+ // Push back into the Vicon Data variable
// if(!outputTranslation.Occluded)
- viconData.crazyflies.push_back(cfData);
+ viconData.crazyflies.push_back(object_data);
}
viconDataPublisher.publish(viconData);
}
@@ -325,25 +342,27 @@ void testFakeData(ros::Publisher viconDataPublisher)
f += 10/1000.0f;
i++;
// TODO: Fake CF data
- CrazyflieData crazyfly;
+ FlyingVehicleState crazyfly;
+
+ crazyfly.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
- crazyfly.occluded = false;
+ crazyfly.isValid = true;
- crazyfly.crazyflieName = "CF1";
+ crazyfly.vehicleName = "CF1";
crazyfly.x = 0;
crazyfly.y = 0;
crazyfly.z = 0;
crazyfly.yaw = 3.14159 * f;
viconData.crazyflies.push_back(crazyfly);
- crazyfly.crazyflieName = "CF2";
+ crazyfly.vehicleName = "CF2";
crazyfly.x = 1;
crazyfly.y = 1;
crazyfly.z = 0;
crazyfly.yaw = -3.14159 * f;
viconData.crazyflies.push_back(crazyfly);
- crazyfly.crazyflieName = "CF3";
+ crazyfly.vehicleName = "CF3";
crazyfly.x = 1;
crazyfly.y = -1;
crazyfly.z = 0;
@@ -352,7 +371,7 @@ void testFakeData(ros::Publisher viconDataPublisher)
if(i > 50 && i < 200)
{
- crazyfly.occluded = true;
+ crazyfly.isValid = true;
}
viconData.crazyflies.push_back(crazyfly);
diff --git a/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py b/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py
index cee7ce819acc2ca2bb836750de3b40ec0278e73c..043d9461ecb2df182e50821502673171584bddae 100755
--- a/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py
+++ b/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py
@@ -71,7 +71,7 @@ from std_msgs.msg import String
# Import the D-FaLL message types
from dfall_pkg.msg import ViconData
-from dfall_pkg.msg import CrazyflieData
+from dfall_pkg.msg import FlyingVehicleState
from dfall_pkg.msg import UnlabeledMarker
# General import
@@ -112,6 +112,11 @@ DEG_TO_RAD = 0.01745329
#QTM_FILE = pkg_resources.resource_filename("qtm", "data/Demo.qtm")
QTM_FILE = "none"
+# Types of flying vehicle states:
+#FLYING_VEHICLE_STATE_TYPE_NONE = 0
+FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT = 1
+#FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE = 2
+#FLYING_VEHICLE_STATE_TYPE_12_STATE_ESTIMATE = 3
# OPEN THE ROS BAG FOR WRITING
#rp = RosPack()
@@ -278,22 +283,25 @@ class QualisysQtmClient:
#print( "rotation = ", rotation )
# Initialise a "ViconData" struct
- # > This is defined in the "CrazyflieData.msg" file
- this_crazyflie_data = CrazyflieData();
+ # > This is defined in the "FlyingVehicleState.msg" file
+ this_crazyflie_data = FlyingVehicleState();
+
+ # Add the type
+ this_crazyflie_data.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
# Add the name of this object
if current_body_index < len(self._list_of_body_names):
- this_crazyflie_data.crazyflieName = self._list_of_body_names[current_body_index]
+ this_crazyflie_data.vehicleName = self._list_of_body_names[current_body_index]
else:
- this_crazyflie_data.crazyflieName = ''
+ this_crazyflie_data.vehicleName = ''
# Check for "nan" as an indication of occulsion
if math.isnan(position.x):
# Fill in the occlusion flag
- this_crazyflie_data.occluded = True
+ this_crazyflie_data.isValid = False
else:
# Fill in the occlusion flag
- this_crazyflie_data.occluded = False
+ this_crazyflie_data.isValid = True
# Fill in the position data
this_crazyflie_data.x = position.x * 0.001
diff --git a/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv b/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv
index 85d54384396813d8517340233eb98bbb3c356a5d..4584a581e1e40879014aff300395cfd12f2e0edc 100755
--- a/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv
+++ b/dfall_ws/src/dfall_pkg/srv/CMQueryCrazyflieName.srv
@@ -1,5 +1,4 @@
string crazyflieName
---
CrazyflieContext crazyflieContext
-uint32 studentID
-
+uint32 studentID
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/srv/Controller.srv b/dfall_ws/src/dfall_pkg/srv/Controller.srv
index 979abc5c4bd047133bd520771cb712443331c16e..c10a6bad937af38985ac9c1d80410ff55d833ffc 100644
--- a/dfall_ws/src/dfall_pkg/srv/Controller.srv
+++ b/dfall_ws/src/dfall_pkg/srv/Controller.srv
@@ -1,5 +1,5 @@
bool isFirstControllerCall
-CrazyflieData ownCrazyflie
-CrazyflieData[] otherCrazyflies
+FlyingVehicleState ownCrazyflie
+FlyingVehicleState[] otherCrazyflies
---
-ControlCommand controlOutput
+ControlCommand controlOutput
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/srv/GetDebugValuesService.srv b/dfall_ws/src/dfall_pkg/srv/GetDebugValuesService.srv
new file mode 100644
index 0000000000000000000000000000000000000000..53783323b02f353692ed1af3f3608520dc9a57bf
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/srv/GetDebugValuesService.srv
@@ -0,0 +1,3 @@
+uint32 data
+---
+DebugMsg values
diff --git a/dfall_ws/src/dfall_pkg/srv/IntStringService.srv b/dfall_ws/src/dfall_pkg/srv/IntStringService.srv
new file mode 100644
index 0000000000000000000000000000000000000000..0c46a893757e9e93c86d10bb798cc6e37d357532
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/srv/IntStringService.srv
@@ -0,0 +1,3 @@
+uint32 data
+---
+string data
\ No newline at end of file
diff --git a/web_interface/html/agent.php b/web_interface/html/agent.php
new file mode 100644
index 0000000000000000000000000000000000000000..6b19f212ddb587e9b57aa7eba4e60362ecaabdda
--- /dev/null
+++ b/web_interface/html/agent.php
@@ -0,0 +1,91 @@
+<?php
+ include("page_header.html");
+?>
+
+
+<script src="js/sse_agentStatus.js?ver=0.1"></script>
+
+
+<div class="full-window-fixed">
+</div>
+
+<div class="max-width-full-heigth-fixed">
+
+ <div class="top-bar-container">
+ <table class="top-bar-buttons-table">
+ <tr>
+ <td class="top-bar-motorsoff-button-cell">
+ <button
+ class="button-push red motorsoff"
+ id="buttonMotorsOffForAgent"
+ onclick="sendRosMessage_outputLabelID('motorsoff', '')"
+ >
+ MOTORS-OFF
+ <div class="div-for-button-highlight-on-touchscreen red"></div>
+ </button>
+ </td>
+ <td class="top-bar-info-button-cell">
+ <button class="button-push blue info" onclick="checkForRosAgent()">
+ i
+ <div class="div-for-button-highlight-on-touchscreen blue"></div>
+ </button>
+ </td>
+ </tr>
+ </table>
+
+ <!--
+ <div class="top-bar-title">
+ Agent (IP <?php echo $_SERVER['REMOTE_ADDR']; ?>)
+ </div>
+ -->
+
+ <div class="top-bar-status-icons padbelow">
+ <div class="on-off-switch type2forstatusbar">
+ <input type="checkbox" id="checkboxTopBarStatus" onchange=checkboxTopBarStatus_changed()><label for="checkboxTopBarStatus"></label>
+ </div>
+ <img id="radio-icon" class="status-icon-radio" src="img/rf_disconnected.png">
+ <img id="battery-icon" class="status-icon-battery" src="img/battery_unavailable.png">
+ <img id="flying-state-icon" class="status-icon-flying-state" src="img/flying_state_unavailable.png">
+ </div>
+ </div>
+
+
+ <div class="main-tabs">
+ <input name="tabs" type="radio" id="main-tab-1" checked="checked" class="main-tab-input"/>
+ <label for="main-tab-1" class="main-tab-label">Code</label>
+ <div class="main-tab-panel">
+ <?php
+ include("agent_tab_code.html");
+ ?>
+ </div>
+
+ <input name="tabs" type="radio" id="main-tab-2" class="main-tab-input"/>
+ <label for="main-tab-2" class="main-tab-label">Compile</label>
+ <div class="main-tab-panel">
+ <?php
+ include("agent_tab_compile.html");
+ ?>
+ </div>
+
+ <input name="tabs" type="radio" id="main-tab-3" class="main-tab-input"/>
+ <label for="main-tab-3" class="main-tab-label">Launch</label>
+ <div class="main-tab-panel">
+ <?php
+ include("agent_tab_launch.html");
+ ?>
+ </div>
+
+ <input name="tabs" type="radio" id="main-tab-4" class="main-tab-input"/>
+ <label for="main-tab-4" class="main-tab-label">Control</label>
+ <div class="main-tab-panel thin-padding">
+ <?php
+ include("agent_tab_control.php");
+ ?>
+ </div>
+ </div>
+</div>
+
+
+<?php
+ include("page_footer.html");
+?>
\ No newline at end of file
diff --git a/web_interface/html/agent_control_tab_default.html b/web_interface/html/agent_control_tab_default.html
new file mode 100644
index 0000000000000000000000000000000000000000..f187738bbf6fadde4f1a5df6227202c80a1c9a92
--- /dev/null
+++ b/web_interface/html/agent_control_tab_default.html
@@ -0,0 +1,327 @@
+<br>
+
+<button
+ class="button-push navy fullwidth"
+ id="buttonControlEnableDefault"
+ onclick="sendRosMessage_outputLabelID('enabledefault', 'labelControlEnableDefault')"
+ >
+ enable
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+</button>
+<div
+ style="text-align: center; display: block;"
+ id="labelControlEnableDefault"
+ >
+  
+</div>
+
+<hr class="hr-basic navy">
+
+<h2 style="text-align: center;">
+ Adjust Setpoint
+</h2>
+
+<table style="margin-left: auto; margin-right: auto;">
+ <tr>
+ <td class="setpoint-label-cell">
+ x [m]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlDefaultSetpointX"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointX', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlDefaultSetpointX"
+ type="text"
+ name="x"
+ value="0.0"
+ step="0.1"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlDefaultSetpointX"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointX', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="setpoint-label-cell">
+ y [m]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlDefaultSetpointY"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointY', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlDefaultSetpointY"
+ type="text"
+ name="y"
+ value="0.0"
+ step="0.1"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlDefaultSetpointY"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointY', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="setpoint-label-cell">
+ z [m]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlDefaultSetpointZ"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointZ', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlDefaultSetpointZ"
+ type="text"
+ name="z"
+ value="0.4"
+ step="0.1"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlDefaultSetpointZ"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointZ', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="setpoint-label-cell">
+ yaw [deg]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlDefaultSetpointYaw"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointYaw', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlDefaultSetpointYaw"
+ type="text"
+ name="yaw"
+ value="0"
+ step="5"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlDefaultSetpointYaw"
+ onclick="incrementInputFieldWithGivenID('inputControlDefaultSetpointYaw', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td></td>
+ <td></td>
+ <td>
+ <button
+ class="button-push navy fullwidth"
+ id="buttonControlDefaultSetSetpoint"
+ onclick="setSetpointViaRosMessage_outputLabelID('default', 'inputControlDefaultSetpoint', 'labelControlDefaultSetSetpoint' )"
+ >
+ set
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td></td>
+ </tr>
+ <tr>
+ <td></td>
+ <td></td>
+ <td>
+ <div
+ style="text-align: center;"
+ id="labelControlDefaultSetSetpoint"
+ >
+  
+ </div>
+ </td>
+ <td></td>
+ </tr>
+</table>
+
+<hr class="hr-basic navy">
+
+<br>
+
+<table class="mse-table">
+ <tr>
+ <td></td>
+ <td class="mse-column-title-cell">
+ <span class="mse-column-title-span-full-text">Measured<br>Pose</span>
+ <span class="mse-column-title-span-short-text">Meas.<br>Pose</span>
+ </td>
+ <td class="mse-column-title-cell">
+ <span class="mse-column-title-span-full-text">Error</span>
+ <span class="mse-column-title-span-short-text">Error</span>
+ </td>
+ <td class="mse-column-title-cell">
+ <span class="mse-column-title-span-full-text">Current<br>Setpoint</span>
+ <span class="mse-column-title-span-short-text">Curr.<br>Setp.</span>
+ </td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ x
+ </td>
+ <td class="mse-data-cell" id="default-measurement-x">xx.xx</td>
+ <td class="mse-data-cell" id="default-error-x">xx.xx</td>
+ <td class="mse-data-cell" id="default-setpoint-x">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ y
+ </td>
+ <td class="mse-data-cell" id="default-measurement-y">xx.xx</td>
+ <td class="mse-data-cell" id="default-error-y">xx.xx</td>
+ <td class="mse-data-cell" id="default-setpoint-y">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ z
+ </td>
+ <td class="mse-data-cell" id="default-measurement-z">xx.xx</td>
+ <td class="mse-data-cell" id="default-error-z">xx.xx</td>
+ <td class="mse-data-cell" id="default-setpoint-z">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ yaw
+ </td>
+ <td class="mse-data-cell" id="default-measurement-yaw">xx.xx</td>
+ <td class="mse-data-cell" id="default-error-yaw">xx.xx</td>
+ <td class="mse-data-cell" id="default-setpoint-yaw">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ pitch
+ </td>
+ <td class="mse-data-cell" id="default-measurement-pitch">xx.xx</td>
+ <td></td>
+ <td></td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ roll
+ </td>
+ <td class="mse-data-cell" id="default-measurement-roll">xx.xx</td>
+ <td></td>
+ <td></td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>NOTES:</b>
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>(x,y,z)</b> in meters
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>(roll,pitch,yaw)</b> in degrees
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>error =</b> measured - setpoint
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>~2Hz</b> data refresh rate
+ </td>
+ </tr>
+</table>
+
+<br>
+
+<hr class="hr-basic navy">
+
+<table class="git-table">
+ <tr>
+ <td class="git-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonLoadYamlDefault"
+ onclick="sendRosMessage_outputLabelID('loadyamldefault', 'labelLoadYamlDefault')"
+ >
+ Load YAML Paramters for Default Controller
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td
+ style="text-align: center;"
+ id="labelLoadYamlDefault"
+ >
+  
+ </td>
+ </tr>
+</table>
+
+<hr class="hr-basic navy">
+
+<br><br><br><br>
\ No newline at end of file
diff --git a/web_interface/html/agent_control_tab_student.html b/web_interface/html/agent_control_tab_student.html
new file mode 100644
index 0000000000000000000000000000000000000000..57f0f48ec7ddce2e7737de7eee568f4b5ca198bc
--- /dev/null
+++ b/web_interface/html/agent_control_tab_student.html
@@ -0,0 +1,410 @@
+<br>
+
+<button
+ class="button-push navy fullwidth"
+ id="buttonControlEnableStudent"
+ onclick="sendRosMessage_outputLabelID('enablestudent', 'labelControlEnableStudent')"
+ >
+ enable
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+</button>
+<div
+ style="text-align: center; display: block;"
+ id="labelControlEnableStudent"
+ >
+  
+</div>
+
+<hr class="hr-basic navy">
+
+<h2 style="text-align: center;">
+ Adjust Setpoint
+</h2>
+
+<table style="margin-left: auto; margin-right: auto;">
+ <tr>
+ <td class="setpoint-label-cell">
+ x [m]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlStudentSetpointX"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointX', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlStudentSetpointX"
+ type="text"
+ name="x"
+ value="0"
+ step="0.1"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlStudentSetpointX"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointX', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="setpoint-label-cell">
+ y [m]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlStudentSetpointY"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointY', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlStudentSetpointY"
+ type="text"
+ name="y"
+ value="0"
+ step="0.1"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlStudentSetpointY"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointY', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="setpoint-label-cell">
+ z [m]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlStudentSetpointZ"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointZ', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlStudentSetpointZ"
+ type="text"
+ name="z"
+ value="0.4"
+ step="0.1"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlStudentSetpointZ"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointZ', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="setpoint-label-cell">
+ yaw [deg]
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonDecrementControlStudentSetpointYaw"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointYaw', -1.0 )"
+ >
+ -
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="setpoint-input-field-cell">
+ <input
+ class="setpoint-input-field"
+ id="inputControlStudentSetpointYaw"
+ type="text"
+ name="yaw"
+ value="0"
+ step="5"
+ pattern="^[-+]?\d*(\.)?(\d?)*"
+ inputmode="none"
+ >
+ </td>
+ <td class="setpoint-increment-cell">
+ <button
+ class="button-push navy slim short10"
+ id="buttonIncrementControlStudentSetpointYaw"
+ onclick="incrementInputFieldWithGivenID('inputControlStudentSetpointYaw', 1.0 )"
+ >
+ +
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td></td>
+ <td></td>
+ <td>
+ <button
+ class="button-push navy fullwidth"
+ id="buttonControlStudentSetSetpoint"
+ onclick="setSetpointViaRosMessage_outputLabelID('student', 'inputControlStudentSetpoint', 'labelControlStudentSetSetpoint' )"
+ >
+ set
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td></td>
+ </tr>
+ <tr>
+ <td></td>
+ <td></td>
+ <td>
+ <div
+ style="text-align: center;"
+ id="labelControlStudentSetSetpoint"
+ >
+  
+ </div>
+ </td>
+ <td></td>
+ </tr>
+</table>
+
+<hr class="hr-basic navy">
+
+<br>
+
+<table class="mse-table">
+ <tr>
+ <td></td>
+ <td class="mse-column-title-cell">
+ <span class="mse-column-title-span-full-text">Measured<br>Pose</span>
+ <span class="mse-column-title-span-short-text">Meas.<br>Pose</span>
+ </td>
+ <td class="mse-column-title-cell">
+ <span class="mse-column-title-span-full-text">Error</span>
+ <span class="mse-column-title-span-short-text">Error</span>
+ </td>
+ <td class="mse-column-title-cell">
+ <span class="mse-column-title-span-full-text">Current<br>Setpoint</span>
+ <span class="mse-column-title-span-short-text">Curr.<br>Setp.</span>
+ </td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ x
+ </td>
+ <td class="mse-data-cell" id="student-measurement-x">xx.xx</td>
+ <td class="mse-data-cell" id="student-error-x">xx.xx</td>
+ <td class="mse-data-cell" id="student-setpoint-x">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ y
+ </td>
+ <td class="mse-data-cell" id="student-measurement-y">xx.xx</td>
+ <td class="mse-data-cell" id="student-error-y">xx.xx</td>
+ <td class="mse-data-cell" id="student-setpoint-y">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ z
+ </td>
+ <td class="mse-data-cell" id="student-measurement-z">xx.xx</td>
+ <td class="mse-data-cell" id="student-error-z">xx.xx</td>
+ <td class="mse-data-cell" id="student-setpoint-z">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ yaw
+ </td>
+ <td class="mse-data-cell" id="student-measurement-yaw">xx.xx</td>
+ <td class="mse-data-cell" id="student-error-yaw">xx.xx</td>
+ <td class="mse-data-cell" id="student-setpoint-yaw">xx.xx</td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ pitch
+ </td>
+ <td class="mse-data-cell" id="student-measurement-pitch">xx.xx</td>
+ <td></td>
+ <td></td>
+ </tr>
+ <tr>
+ <td class="mse-row-title-cell">
+ roll
+ </td>
+ <td class="mse-data-cell" id="student-measurement-roll">xx.xx</td>
+ <td></td>
+ <td></td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>NOTES:</b>
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>(x,y,z)</b> in meters
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>(roll,pitch,yaw)</b> in degrees
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>error =</b> measured - setpoint
+ </td>
+ </tr>
+ <tr>
+ <td colspan="4" class="mse-caption-below">
+ <b>~2Hz</b> data refresh rate
+ </td>
+ </tr>
+</table>
+
+<br>
+
+<hr class="hr-basic navy">
+
+<table class="git-table">
+ <tr>
+ <td class="git-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonLoadYamlStudent"
+ onclick="sendRosMessage_outputLabelID('loadyamlstudent', 'labelLoadYamlStudent')"
+ >
+ Load YAML Paramters for Student Controller
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td
+ style="text-align: center;"
+ id="labelLoadYamlStudent"
+ >
+  
+ </td>
+ </tr>
+</table>
+
+<hr class="hr-basic navy">
+
+<br>
+
+<h2 style="text-align: center;">
+ Debug Values
+</h2>
+
+<table class="debug-values-table">
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 1
+ </td>
+ <td class="debug-values-data-cell" id="debug-value1">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 2
+ </td>
+ <td class="debug-values-data-cell" id="debug-value2">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 3
+ </td>
+ <td class="debug-values-data-cell" id="debug-value3">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 4
+ </td>
+ <td class="debug-values-data-cell" id="debug-value4">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 5
+ </td>
+ <td class="debug-values-data-cell" id="debug-value5">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 6
+ </td>
+ <td class="debug-values-data-cell" id="debug-value6">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 7
+ </td>
+ <td class="debug-values-data-cell" id="debug-value7">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 8
+ </td>
+ <td class="debug-values-data-cell" id="debug-value8">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 9
+ </td>
+ <td class="debug-values-data-cell" id="debug-value9">xx.xx</td>
+ </tr>
+ <tr>
+ <td></td>
+ <td class="debug-values-row-title-cell">
+ Value 10
+ </td>
+ <td class="debug-values-data-cell" id="debug-value10">xx.xx</td>
+ </tr>
+</table>
+
+<br>
+
+<hr class="hr-basic navy">
+
+<br><br><br><br>
\ No newline at end of file
diff --git a/web_interface/html/agent_tab_code.html b/web_interface/html/agent_tab_code.html
new file mode 100644
index 0000000000000000000000000000000000000000..b93f163e3568906b9504a7038cdb123cb8891f78
--- /dev/null
+++ b/web_interface/html/agent_tab_code.html
@@ -0,0 +1,79 @@
+<body>
+ <div class="body-container">
+ <main style="text-align:left;">
+
+ <br>
+
+ <form action="upload.php" method="post" enctype="multipart/form-data">
+ Upload your code below.
+ <br>
+ <br>
+ <input class="file" type="file" id="myToUpload" name="fileToUpload">
+ <br>
+ <input type="submit" value="Upload Code" name="submit">
+ </form>
+
+ <br><br><br>
+
+
+ <input type="hidden" id="MAX_FILE_SIZE" name="MAX_FILE_SIZE" value="1000000" />
+
+
+ <hr class="hr-basic navy">
+
+ <label for="cppfileinput" class="file-input-label">
+ Select your StudentControllerService.<b>cpp</b> file
+ </label>
+ <br>
+ <form id="cppuploadform" action="upload_async.php" method="POST" enctype="multipart/form-data">
+ <input type="file" id="cppfileinput" name="cppfile" class="file-input" />
+ </form>
+
+ <br>
+
+ <div id="cppfileuploadstatus" class="file-upload-status-div">
+  
+ </div>
+
+ <br>
+
+ <div id="cppfiledetails" class="file-input-details-div">
+  
+ </div>
+
+ <br>
+
+ <hr class="hr-basic navy">
+
+ <label for="yamlfileinput" class="file-input-label">
+ Select your StudentControllerService.<b>yaml</b> file
+ </label>
+ <br>
+ <form id="yamluploadform" action="upload_async.php" method="POST" enctype="multipart/form-data">
+ <input type="file" id="yamlfileinput" name="yamlfile" class="file-input" />
+ </form>
+
+ <br>
+
+ <div id="yamlfileuploadstatus" class="file-upload-status-div">
+  
+ </div>
+
+ <br>
+
+ <div id="yamlfiledetails" class="file-input-details-div">
+  
+ </div>
+
+ <br>
+
+ <hr class="hr-basic navy">
+
+ <br><br><br>
+
+ <div id="agentStatusDebuggingDiv"></div>
+
+ <br><br>
+ </main>
+ </div>
+</body>
\ No newline at end of file
diff --git a/web_interface/html/agent_tab_compile.html b/web_interface/html/agent_tab_compile.html
new file mode 100644
index 0000000000000000000000000000000000000000..f2f1bc60174f2d3f77e26970ef694024bbeeb4be
--- /dev/null
+++ b/web_interface/html/agent_tab_compile.html
@@ -0,0 +1,112 @@
+<body>
+ <div class="body-container">
+ <main style="text-align:left;">
+
+ <br>
+
+ <table class="git-table">
+ <tr>
+ <td class="git-button-cell left">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonGitStatus"
+ onclick="callGit_outputLabelID_shouldAppend_shouldConfirm('status', 'terminalOutput', true, false)"
+ >
+ git status
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="git-button-cell right">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonGitPull"
+ onclick="callGit_outputLabelID_shouldAppend_shouldConfirm('pull', 'terminalOutput', true, false)"
+ >
+ git pull
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td class="git-button-cell left">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonGitDiff"
+ onclick="callGit_outputLabelID_shouldAppend_shouldConfirm('diff', 'terminalOutput', true, false)"
+ >
+ git diff
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="git-button-cell right">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonCatkinMake"
+ onclick="callGit_outputLabelID_shouldAppend_shouldConfirm('catkin_make', 'terminalOutput', false, false)"
+ >
+ catkin make
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ <!-- ALTERNATIVE FOR THIS BUTTON -->
+ <!-- onclick="callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons('catkin_make', 'terminalOutput' , null, null)" -->
+ </td>
+ </tr>
+ </table>
+
+ <div class="git-checkout-label">git checkout</div>
+ <table class="git-table">
+ <tr>
+ <td class="git-button-cell left">
+ <button
+ class="button-push red fullwidth"
+ id="buttonGitCheckoutAll"
+ onclick="callGit_outputLabelID_shouldAppend_shouldConfirm('checkoutall', 'terminalOutput', true, true)"
+ >
+ all
+ <div class="div-for-button-highlight-on-touchscreen red"></div>
+ </button>
+ </td>
+ <td class="git-button-cell right">
+ <button
+ class="button-push red fullwidth"
+ id="buttonGitCheckoutMaster"
+ onclick="callGit_outputLabelID_shouldAppend_shouldConfirm('checkoutmaster', 'terminalOutput', true, true)"
+ >
+ master
+ <div class="div-for-button-highlight-on-touchscreen red"></div>
+ </button>
+ </td>
+ </tr>
+ </table>
+
+ <br><br>
+
+
+
+ <table class="terminal-label-table">
+ <tr>
+ <td class="terminal-label-cell">
+ <div class="terminal-label">TERMINAL OUTPUT</div>
+ </td>
+ <td>
+ <button
+ class="button-push navy monospace short"
+ id="buttonGitCheckout"
+ onclick="clearLabelWithGivenID('terminalOutput')"
+ >
+ clear
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ </table>
+
+ <div class="terminal-background">
+ <div class="terminal-contents" id="terminalOutput"></div>
+ </div>
+
+ <br>
+
+ </main>
+ </div>
+</body>
\ No newline at end of file
diff --git a/web_interface/html/agent_tab_control.php b/web_interface/html/agent_tab_control.php
new file mode 100644
index 0000000000000000000000000000000000000000..77f93a7a5eb3c8308cfdbfed2dd07ba4790367a1
--- /dev/null
+++ b/web_interface/html/agent_tab_control.php
@@ -0,0 +1,97 @@
+<table class="git-table">
+ <tr>
+ <td class="git-button-cell left">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonControlRadioDisconnect"
+ onclick="sendRosMessage_outputLabelID('disconnect', 'labelControlRadioDisconnect')"
+ >
+ disconnect
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="git-button-cell left">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonControlRadioConnect"
+ onclick="sendRosMessage_outputLabelID('connect', 'labelControlRadioConnect')"
+ >
+ connect
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td
+ style="text-align: center;"
+ id="labelControlRadioDisconnect"
+ >
+  
+ </td>
+ <td
+ style="text-align: center;"
+ id="labelControlRadioConnect"
+ >
+  
+ </td>
+ </tr>
+ <tr>
+ <td class="git-button-cell left">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonControlTakeoff"
+ onclick="sendRosMessage_outputLabelID('takeoff', 'labelControlTakeOff')"
+ >
+ take-off
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="git-button-cell left">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonControlLand"
+ onclick="sendRosMessage_outputLabelID('land', 'labelControlLand')"
+ >
+ land
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ </tr>
+ <tr>
+ <td
+ style="text-align: center;"
+ id="labelControlTakeOff"
+ >
+  
+ </td>
+ <td
+ style="text-align: center;"
+ id="labelControlLand"
+ >
+  
+ </td>
+ </tr>
+</table>
+
+<br>
+
+<div class="control-tabs">
+ <input name="control-tabs" type="radio" id="control-tab-1" checked="checked" class="control-tab-input"/>
+ <label for="control-tab-1" class="control-tab-label">Default</label>
+ <div class="control-tab-panel" id="control-tab-panel-default">
+ <?php
+ include("agent_control_tab_default.html");
+ ?>
+ </div>
+
+ <input name="control-tabs" type="radio" id="control-tab-2" class="control-tab-input"/>
+ <label for="control-tab-2" class="control-tab-label">Student</label>
+ <div class="control-tab-panel" id="control-tab-panel-student">
+ <?php
+ include("agent_control_tab_student.html");
+ ?>
+ </div>
+
+</div>
+
+<br>
\ No newline at end of file
diff --git a/web_interface/html/agent_tab_launch.html b/web_interface/html/agent_tab_launch.html
new file mode 100644
index 0000000000000000000000000000000000000000..06c04845ae5b0578a2d69e209fdbb3d2f98a20c1
--- /dev/null
+++ b/web_interface/html/agent_tab_launch.html
@@ -0,0 +1,179 @@
+<body>
+ <div class="body-container">
+ <main style="text-align:left;">
+
+ <br>
+
+ <table class="ros-table">
+
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonRosMasterStatus"
+ onclick="callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons('checkForRosMaster', 'rosMasterStatus', ['rosLaunchMasterStatus','killMasterStatus'], null)"
+ >
+ Check for ROS Master
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="ros-info-cell">
+ <div id="rosMasterStatus"></div>
+ </td>
+ </tr>
+
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonLaunchRosMaster"
+ onclick="roslaunch_outputLabelID_clearOtherLabels_clickOtherButtons('master', 'rosLaunchMasterStatus', ['rosMasterStatus','killMasterStatus'], ['buttonRosAgentStatus','buttonRosNodeList'])"
+ >
+ Launch ROS Master
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="ros-info-cell">
+ <div id="rosLaunchMasterStatus"></div>
+ </td>
+ </tr>
+ <tr>
+ <td>
+ <table>
+ <td class="switch-cell pad-right">
+ <div class="on-off-switch type4">
+ <input type="checkbox" id="checkboxEmulateMocap"><label for="checkboxEmulateMocap"></label>
+ </div>
+ </td>
+ <td class="ros-info-cell">
+ Emulate motion capture
+ </td>
+ </table>
+ </td>
+ <td class="ros-info-cell">
+  
+ </td>
+ </tr>
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push red fullwidth"
+ id="buttonKillRosMaster"
+ onclick="callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons('killRosMaster', 'killMasterStatus', ['rosMasterStatus','rosLaunchMasterStatus'], ['buttonRosAgentStatus','buttonRosNodeList'])"
+ >
+ Kill ROS Master
+ <div class="div-for-button-highlight-on-touchscreen red"></div>
+ </button>
+ </td>
+ <td class="ros-info-cell">
+ <div id="killMasterStatus"></div>
+ </td>
+ </tr>
+
+ </table>
+
+ <br>
+ <hr class="hr-basic navy">
+ <br>
+
+ <table class="ros-table">
+
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonRosAgentStatus"
+ onclick="callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons('checkForRosAgent', 'rosAgentStatus', ['rosLaunchAgentStatus','killAgentStatus'], 'buttonRosNodeList')"
+ >
+ Check for ROS Agent
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="ros-info-cell">
+ <div id="rosAgentStatus"></div>
+ </td>
+ </tr>
+
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id="buttonLaunchRosAgent"
+ onclick="roslaunch_outputLabelID_clearOtherLabels_clickOtherButtons('agent', 'rosLaunchAgentStatus', ['rosAgentStatus','killAgentStatus'], 'buttonRosNodeList')"
+ >
+ Launch ROS Agent
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td class="ros-info-cell">
+ <div id="rosLaunchAgentStatus"></div>
+ </td>
+ </tr>
+ <tr>
+ <td>
+ <table>
+ <td class="switch-cell pad-right">
+ <div class="on-off-switch type4">
+ <input type="checkbox" id="checkboxEmulateCrazyRadio"><label for="checkboxEmulateCrazyRadio"></label>
+ </div>
+ </td>
+ <td class="ros-info-cell">
+ Emulate CrazyRadio
+ </td>
+ </table>
+ </td>
+ <td class="ros-info-cell">
+  
+ </td>
+ </tr>
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push red fullwidth"
+ id="buttonKillRosAgent"
+ onclick="callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons('killRosAgent', 'killAgentStatus', ['rosAgentStatus','rosLaunchAgentStatus'], 'buttonRosNodeList')"
+ >
+ Kill ROS Agent
+ <div class="div-for-button-highlight-on-touchscreen red"></div>
+ </button>
+ </td>
+ <td class="ros-info-cell">
+ <div id="killAgentStatus"></div>
+ </td>
+ </tr>
+
+ </table>
+
+ <br>
+ <hr class="hr-basic navy">
+ <br>
+
+ <table class="ros-table">
+
+ <tr>
+ <td class="ros-button-cell">
+ <button
+ class="button-push navy fullwidth"
+ id ="buttonRosNodeList"
+ onclick="callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons('rosnodeList', 'rosnodeList', null, null)"
+ >
+ List ROS Nodes
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+ </td>
+ <td></td>
+ </tr>
+
+ </table>
+
+ <table class="ros-table">
+ <tr>
+ <td class="ros-info-cell" style="height:100px;vertical-align:text-top;">
+ <div id="rosnodeList"></div>
+ </td>
+ </tr>
+ </table>
+
+ </main>
+ </div>
+</body>
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/catkin_make.sh b/web_interface/html/bashscripts/catkin_make.sh
new file mode 100755
index 0000000000000000000000000000000000000000..96e9494ba1d7b485ca9998655113c87f3f617cff
--- /dev/null
+++ b/web_interface/html/bashscripts/catkin_make.sh
@@ -0,0 +1,13 @@
+#!/bin/bash
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+#source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/dfall_ws
+#
+# Call catkin_make
+catkin_make
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/catkin_make_clean.sh b/web_interface/html/bashscripts/catkin_make_clean.sh
new file mode 100755
index 0000000000000000000000000000000000000000..1d6935cb9ccbd360d4e8604ebf6397d8e32e24cd
--- /dev/null
+++ b/web_interface/html/bashscripts/catkin_make_clean.sh
@@ -0,0 +1,13 @@
+#!/bin/bash
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+#source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/dfall_ws
+#
+# Call catkin_make
+catkin_make clean
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/checkForRosAgent.sh b/web_interface/html/bashscripts/checkForRosAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..27ef70f78d51d505eda246a3990eaeb2397d4c9e
--- /dev/null
+++ b/web_interface/html/bashscripts/checkForRosAgent.sh
@@ -0,0 +1,21 @@
+#!/bin/bash
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+#source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Check if the agent exists
+ if rosnode list | grep -q "$(printf "/dfall/agent%03d" $DFALL_DEFAULT_AGENT_ID)"; then
+ echo "Agent $DFALL_DEFAULT_AGENT_ID exists"
+ else
+ echo "Agent $DFALL_DEFAULT_AGENT_ID not found"
+ fi
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/checkForRosMaster.sh b/web_interface/html/bashscripts/checkForRosMaster.sh
new file mode 100755
index 0000000000000000000000000000000000000000..285240e74a28b08a95342273317cd6e5cf7f0b2e
--- /dev/null
+++ b/web_interface/html/bashscripts/checkForRosMaster.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+#
+# Perform the check
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ echo "ROS Master exists"
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/gitCheckoutAll.sh b/web_interface/html/bashscripts/gitCheckoutAll.sh
new file mode 100755
index 0000000000000000000000000000000000000000..013f312c4ad88d824d2704de30804f7e159b322c
--- /dev/null
+++ b/web_interface/html/bashscripts/gitCheckoutAll.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/
+#
+# Call git
+git checkout .
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/gitCheckoutMaster.sh b/web_interface/html/bashscripts/gitCheckoutMaster.sh
new file mode 100755
index 0000000000000000000000000000000000000000..b0320528b7a7d9f4016d8b9bdc762a5547f95966
--- /dev/null
+++ b/web_interface/html/bashscripts/gitCheckoutMaster.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/
+#
+# Call git
+git checkout master
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/gitDiff.sh b/web_interface/html/bashscripts/gitDiff.sh
new file mode 100755
index 0000000000000000000000000000000000000000..bb2873913a4623f1b6405c16851dbb1acce0d6bd
--- /dev/null
+++ b/web_interface/html/bashscripts/gitDiff.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/
+#
+# Call git
+git diff
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/gitPull.sh b/web_interface/html/bashscripts/gitPull.sh
new file mode 100755
index 0000000000000000000000000000000000000000..8849eb92815478f51126729ef5e9f90942b25820
--- /dev/null
+++ b/web_interface/html/bashscripts/gitPull.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/
+#
+# Call git
+sudo -u www-data git pull
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/gitStatus.sh b/web_interface/html/bashscripts/gitStatus.sh
new file mode 100755
index 0000000000000000000000000000000000000000..5e60e9beb6d2a5f2c5c7624e4d70506a97ba96a3
--- /dev/null
+++ b/web_interface/html/bashscripts/gitStatus.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+#
+# Change directory to the dfall-system repository
+cd /home/www-share/dfall/dfall-system/
+#
+# Call git
+git status
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/killRosAgent.sh b/web_interface/html/bashscripts/killRosAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..1b687066cdf539004b42fc6a15f7dd9f48025a25
--- /dev/null
+++ b/web_interface/html/bashscripts/killRosAgent.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+#source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Check that if the agent is already
+ # launched
+ if rosnode list | grep -q "$(printf "/dfall/agent%03d" $DFALL_DEFAULT_AGENT_ID)"; then
+ # Kill all nodes starting with
+ # /dfall/agentXXX/
+ rosnode list | grep "$(printf "/dfall/agent%03d" $DFALL_DEFAULT_AGENT_ID)" | xargs rosnode kill > /dev/null
+ echo "Agent $DFALL_DEFAULT_AGENT_ID killed"
+ else
+ echo "Agent $DFALL_DEFAULT_AGENT_ID not found"
+ fi
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/killRosMaster.sh b/web_interface/html/bashscripts/killRosMaster.sh
new file mode 100755
index 0000000000000000000000000000000000000000..8945b1caed5f16e39f3a5d8105a9848cb9e97010
--- /dev/null
+++ b/web_interface/html/bashscripts/killRosMaster.sh
@@ -0,0 +1,27 @@
+#!/bin/bash
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+#source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Kill all nodes
+ rosnode kill --all > /dev/null 2>&1 &
+ # The /rosout will respawn, so all kill any
+ # "roslaunch" process owned by the
+ # "www-data" user
+ pkill -u www-data roslaunch
+ # For debugging it is usefull to running the
+ # following commands on the server machine
+ # >> ps -aux
+ # >> pgrep -u www-data
+ # Inform the user
+ echo "Killed all ros nodes"
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/launchRosAgent.sh b/web_interface/html/bashscripts/launchRosAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..a2ba643f4b1c0480cfd061e8b6417a57222ebaa0
--- /dev/null
+++ b/web_interface/html/bashscripts/launchRosAgent.sh
@@ -0,0 +1,44 @@
+#!/bin/bash
+#
+# Check that exactly one command is supplied
+if [ "$#" -ne 1 ]; then
+ echo "exactly one argument must be supplied"
+ exit 1
+fi
+#
+# Check that the first command supplied is valid
+if [ "$1" != "true" ] && [ "$1" != "false" ]; then
+ echo "emulate crazyradio = $1, is not a valid boolean."
+ exit 1
+fi
+#
+# Put the commands into variables for make things more readable
+emulateradio=$1
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Set the ROS log location
+export ROS_LOG_DIR=/var/www/html/.ros/log
+#
+# Mount the dfall workspace folder
+#sudo mount --bind /home/pbeuchat/gitrep/dfall/dfall-system/dfall_ws /var/www/html/dfall_ws
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Check that if the agent is already
+ # launched
+ if rosnode list | grep -q "$(printf "/dfall/agent%03d" $DFALL_DEFAULT_AGENT_ID)"; then
+ echo "Agent $DFALL_DEFAULT_AGENT_ID is already running"
+ else
+ nohup roslaunch dfall_pkg pi_agent.launch emulateRadio:=$emulateradio > /dev/null 2>&1 &
+ echo "Agent $DFALL_DEFAULT_AGENT_ID successfully launched"
+ fi
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/launchRosMaster.sh b/web_interface/html/bashscripts/launchRosMaster.sh
new file mode 100755
index 0000000000000000000000000000000000000000..a6b95473de16f8f038b32d60d1f1d7573bc88b33
--- /dev/null
+++ b/web_interface/html/bashscripts/launchRosMaster.sh
@@ -0,0 +1,38 @@
+#!/bin/bash
+#
+# Check that exactly one command is supplied
+if [ "$#" -ne 1 ]; then
+ echo "exactly one argument must be supplied"
+ exit 1
+fi
+#
+# Check that the first command supplied is valid
+if [ "$1" != "true" ] && [ "$1" != "false" ]; then
+ echo "emulate mocap = $1, is not a valid boolean."
+ exit 1
+fi
+#
+# Put the commands into variables for make things more readable
+emultemocap=$1
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Set the ROS log location
+export ROS_LOG_DIR=/var/www/html/.ros/log
+#
+# Mount the dfall workspace folder
+#sudo mount --bind /home/pbeuchat/gitrep/dfall/dfall-system/dfall_ws /var/www/html/dfall_ws
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ echo "ROS Master is already running"
+else
+ nohup roslaunch dfall_pkg pi_master.launch emulateMocap:=$emultemocap > /dev/null 2>&1 &
+ echo "ROS Master successfully launched"
+fi
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/rosCrazyRadioCommand_forAgent.sh b/web_interface/html/bashscripts/rosCrazyRadioCommand_forAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..1d826fd3e511fcea4340463145d149d7838eeb98
--- /dev/null
+++ b/web_interface/html/bashscripts/rosCrazyRadioCommand_forAgent.sh
@@ -0,0 +1,49 @@
+#!/bin/bash
+#
+# Check that exactly one command is supplied
+if [ "$#" -ne 1 ]; then
+ echo "exactly one argument must be supplied"
+ exit 1
+fi
+#
+# Check that the command supplied is valid
+if [ "$1" != "0" ] && [ "$1" != "1" ]; then
+ echo "command = $1, is not a valid option"
+ exit 1
+fi
+#
+# Put the command into a variable for make things more readable
+command=$1
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Convert the agent ID to a zero padded string
+ agentnamespace=$(printf "agent%03d" $DFALL_DEFAULT_AGENT_ID)
+ # Send the message
+ if [ "$command" == "0" ]; then
+ # Publish the request
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/CrazyRadioCommand dfall_pkg/IntWithHeader "{data: 0, shouldCheckForAgentID: False}")"
+ # Return that the message was sent
+ echo "sent"
+ #
+ elif [ "$command" == "1" ]; then
+ # Publish the request
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/CrazyRadioCommand dfall_pkg/IntWithHeader "{data: 1, shouldCheckForAgentID: False}")"
+ # Return that the message was sent
+ echo "sent"
+ else
+ # Return that the command is not recognised
+ echo "command = $command is not a valid option"
+ fi
+
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/rosFlyingStateCommand_forAgent.sh b/web_interface/html/bashscripts/rosFlyingStateCommand_forAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..7564ca5b9e7642e6017211fd1969de701b1bfcc7
--- /dev/null
+++ b/web_interface/html/bashscripts/rosFlyingStateCommand_forAgent.sh
@@ -0,0 +1,69 @@
+#!/bin/bash
+#
+# Check that exactly one command is supplied
+if [ "$#" -ne 1 ]; then
+ echo "failed"
+ exit 1
+fi
+#
+# Check that the command supplied is valid
+if [ "$1" != "1" ] && [ "$1" != "2" ] && [ "$1" != "3" ] && [ "$1" != "4" ] && [ "$1" != "5" ] && [ "$1" != "6" ] && [ "$1" != "7" ] && [ "$1" != "8" ] && [ "$1" != "11" ] && [ "$1" != "12" ] && [ "$1" != "13" ]; then
+ echo "failed"
+ exit 1
+fi
+#
+# Put the command into a variable for make things more readable
+command=$1
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Convert the agent ID to a zero padded string
+ agentnamespace=$(printf "agent%03d" $DFALL_DEFAULT_AGENT_ID)
+ # Send the message
+ if [ "$command" == "1" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 1, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "2" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 2, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "3" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 3, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "4" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 4, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "5" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 5, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "6" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 6, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "7" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 7, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "8" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 8, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "11" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 11, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "12" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 12, shouldCheckForAgentID: False}')"
+ fi
+ if [ "$command" == "13" ]; then
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/FlyingAgentClient/Command dfall_pkg/IntWithHeader '{data: 13, shouldCheckForAgentID: False}')"
+ fi
+ #
+ # Return that the message was sent
+ echo "sent"
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/rosGetCurrentSetpoint_forAgent.sh b/web_interface/html/bashscripts/rosGetCurrentSetpoint_forAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..e7ce23f355c0d6ef16cf9ee3cb4d2e3bd9f5260f
--- /dev/null
+++ b/web_interface/html/bashscripts/rosGetCurrentSetpoint_forAgent.sh
@@ -0,0 +1,42 @@
+#!/bin/bash
+#
+# Check that exactly one command is supplied
+if [ "$#" -ne 1 ]; then
+ echo "failed"
+ exit 1
+fi
+#
+# Check that the command supplied is valid
+if [ "$1" != "default" ] && [ "$1" != "student" ]; then
+ echo "failed"
+ exit 1
+fi
+#
+# Put the command into a variable for make things more readable
+command=$1
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Convert the agent ID to a zero padded string
+ agentnamespace=$(printf "agent%03d" $DFALL_DEFAULT_AGENT_ID)
+ # Send the message
+ if [ "$command" == "default" ]; then
+ temp="$(rosservice call /$ROS_NAMESPACE/$agentnamespace/DefaultControllerService/GetCurrentSetpoint 0)"
+ fi
+ if [ "$command" == "student" ]; then
+ temp="$(rosservice call /$ROS_NAMESPACE/$agentnamespace/StudentControllerService/GetCurrentSetpoint 0)"
+ fi
+ #
+ # Return that the message was sent
+ echo "$temp"
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/rosGetStatusJson_forAgent.sh b/web_interface/html/bashscripts/rosGetStatusJson_forAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..8b2d573e3de679ffbbc3fb87c5c1940879d46b54
--- /dev/null
+++ b/web_interface/html/bashscripts/rosGetStatusJson_forAgent.sh
@@ -0,0 +1,82 @@
+#!/bin/bash
+#
+#
+# Check whether a command is supplied
+# > and put it into a more redable variable
+if [ "$#" -eq 0 ]; then
+ eventid="0"
+elif [ "$#" -eq 1 ]; then
+ eventid=$1
+else
+ echo "not allowed to supply two or more arguments"
+ exit 1
+fi
+#
+# Check that the command supplied is valid
+if ! [[ $eventid =~ ^[0-9]*$ ]]; then
+ echo "event id must be an integer, event id = $1"
+ exit 1
+fi
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Check if the agent exists
+ if rosnode list | grep -q "$(printf "/dfall/agent%03d" $DFALL_DEFAULT_AGENT_ID)"; then
+ # Set a flag that agent was found
+ agentfound="true"
+ # Convert the agent ID to a zero padded string
+ agentnamespace=$(printf "agent%03d" $DFALL_DEFAULT_AGENT_ID)
+ # Perform the service call for the status json
+ statusjsonfull="$(rosservice call /$ROS_NAMESPACE/$agentnamespace/AgentStatusForWebInterface/StatusAsJson 0)"
+ # Cut out the "data: " from the start, and the single " at the end
+ statusjsoncut1="${statusjsonfull:7:${#statusjsonfull}-8}"
+ # Remove all new line characters followed by 2 spaces
+ statusjsoncut2=$(echo $statusjsoncut1|tr -d '\n ')
+ # Remove all remaining new line characters
+ statusjsoncut3=$(echo $statusjsoncut2|tr -d '\n')
+ #statusjsoncut3=${statusjsoncut2//$'\n'/}
+ # Remove all carriage return characters
+ statusjsoncut4=$(echo $statusjsoncut3|tr -d '\r')
+ # Remove all tab characters
+ statusjsoncut5=$(echo $statusjsoncut4|tr -d '\t')
+ # Remove all remaining \ escape characters
+ statusjson=${statusjsoncut5//\\}
+ #
+ # Check that the status json is valid based on length
+ if [ ${#statusjsonfull} -le 1 ]; then
+ # Set a flag that agent was NOT found
+ agentfound="false"
+ # Set all other values to "not available (na)"
+ statusjson="\"na\""
+ fi
+ #
+ else
+ # Set a flag that agent was NOT found
+ agentfound="false"
+ # Set all other values to "not available (na)"
+ statusjson="\"na\""
+ fi
+else
+ # Set a flag that agent was NOT found
+ agentfound="false"
+ # Set all other values to "not available (na)"
+ statusjson="\"na\""
+fi
+
+# Return that the values collected
+echo "{"\
+ "\"id\": \"$eventid\","\
+ "\"agentfound\": \"$agentfound\","\
+ "\"statusjson\": $statusjson"\
+ "}"
+
+# DEBUGGING: For testing directly in terminal
+# rosservice call /dfall/agent001/AgentStatusForWebInterface/StatusAsJson 0
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/rosLoadYaml_forAgent.sh b/web_interface/html/bashscripts/rosLoadYaml_forAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..d2b2ecbed5f50179201c75a1a88159b2fd7b67b4
--- /dev/null
+++ b/web_interface/html/bashscripts/rosLoadYaml_forAgent.sh
@@ -0,0 +1,54 @@
+#!/bin/bash
+#
+# Check that exactly one command is supplied
+if [ "$#" -ne 1 ]; then
+ echo "failed"
+ exit 1
+fi
+#
+# Check that the command supplied is valid
+if [ "$1" != "default" ] && [ "$1" != "student" ]; then
+ echo "failed"
+ exit 1
+fi
+#
+# Put the command into a variable for make things more readable
+command=$1
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Check if the agent exists
+ if rosnode list | grep -q "$(printf "/dfall/agent%03d" $DFALL_DEFAULT_AGENT_ID)"; then
+ # Convert the agent ID to a zero padded string
+ agentnamespace=$(printf "agent%03d" $DFALL_DEFAULT_AGENT_ID)
+ # Send the message
+ if [ "$command" == "default" ]; then
+ # Publish the request
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/ParameterService/requestLoadYamlFilename dfall_pkg/StringWithHeader "{data: DefaultController, shouldCheckForAgentID: False}")"
+ # Return that the message was sent
+ echo "sent"
+ #
+ elif [ "$command" == "student" ]; then
+ # Publish the request
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/ParameterService/requestLoadYamlFilename dfall_pkg/StringWithHeader "{data: StudentController, shouldCheckForAgentID: False}")"
+ # Return that the message was sent
+ echo "sent"
+ #
+ else
+ # Return that the command is not recognised
+ echo "controller = $command is not a valid option"
+ fi
+ else
+ echo "Agent $DFALL_DEFAULT_AGENT_ID not found"
+ fi
+else
+ echo "ROS Master not found"
+fi
diff --git a/web_interface/html/bashscripts/rosSetNewSetpoint_forAgent.sh b/web_interface/html/bashscripts/rosSetNewSetpoint_forAgent.sh
new file mode 100755
index 0000000000000000000000000000000000000000..26fd87560ea3b069324d033f35e23bd4b160de68
--- /dev/null
+++ b/web_interface/html/bashscripts/rosSetNewSetpoint_forAgent.sh
@@ -0,0 +1,79 @@
+#!/bin/bash
+#
+# Check that exactly five commands are supplied
+if [ "$#" -ne 5 ]; then
+ echo "exaclty five arguments must be supplied"
+ exit 1
+fi
+#
+# Check that the first command supplied is valid
+if [ "$1" != "default" ] && [ "$1" != "student" ]; then
+ echo "command = $1, is not a valid option."
+ exit 1
+fi
+#
+# Put the commands into variables for make things more readable
+command=$1
+xnew=$2
+ynew=$3
+znew=$4
+yawnew=$5
+#
+# Check that the new setpoint values are purely numerical
+# > For x:
+if ! [[ $xnew =~ ^[+-]?[0-9]+\.?[0-9]*$ ]];then
+ echo "x = $xnew, is not a float."
+ exit 1
+fi
+# > For y:
+if ! [[ $ynew =~ ^[+-]?[0-9]+\.?[0-9]*$ ]];then
+ echo "y = $ynew, is not a float."
+ exit 1
+fi
+# > For z:
+if ! [[ $znew =~ ^[+-]?[0-9]+\.?[0-9]*$ ]];then
+ echo "z = $znew, is not a float."
+ exit 1
+fi
+# > For yaw:
+if ! [[ $yawnew =~ ^[+-]?[0-9]+\.?[0-9]*$ ]];then
+ echo "yaw = $yawnew, is not a float."
+ exit 1
+fi
+#
+# Make the ROS commands available
+# NOTE: these paths should NOT use ~
+source /opt/ros/melodic/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/devel/setup.bash
+source /home/www-share/dfall/dfall-system/dfall_ws/src/dfall_pkg/launch/Config.sh
+#
+# Check that the ROS Master exists
+# > Note: the -q options converts the
+# grep output to a true/false
+if rosnode list | grep -q /rosout; then
+ # Convert the agent ID to a zero padded string
+ agentnamespace=$(printf "agent%03d" $DFALL_DEFAULT_AGENT_ID)
+ # Send the message
+ if [ "$command" == "default" ]; then
+ # Publish the request
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/DefaultControllerService/RequestSetpointChange dfall_pkg/SetpointWithHeader "{x: $xnew, y: $ynew, z: $znew, yaw: $yawnew, shouldCheckForAgentID: False}")"
+ # Return that the message was sent
+ echo "sent"
+ #
+ elif [ "$command" == "student" ]; then
+ # Publish the request
+ temp="$(rostopic pub -1 /$ROS_NAMESPACE/$agentnamespace/StudentControllerService/RequestSetpointChange dfall_pkg/SetpointWithHeader "{x: $xnew, y: $ynew, z: $znew, yaw: $yawnew, shouldCheckForAgentID: False}")"
+ # Return that the message was sent
+ echo "sent"
+ #
+ else
+ # Return that the command is not recognised
+ echo "controller = $command is not a valid option"
+ fi
+
+else
+ echo "ROS Master not found"
+fi
+
+# DEBUGGING: For testing directly in terminal
+# rostopic pub -1 /dfall/agent001/DefaultControllerService/RequestSetpointChange dfall_pkg/SetpointWithHeader {x: 0.0, y: 0.0, z: 0.1, yaw: 0, shouldCheckForAgentID: False}
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/roscore.sh b/web_interface/html/bashscripts/roscore.sh
new file mode 100755
index 0000000000000000000000000000000000000000..36be7a3807b3208d28012c7b89fefeb7977649fe
--- /dev/null
+++ b/web_interface/html/bashscripts/roscore.sh
@@ -0,0 +1,5 @@
+#!/bin/bash
+echo "Now attempting roscore"
+source /opt/ros/melodic/setup.bash
+roscore
+echo "...attemp complete."
diff --git a/web_interface/html/bashscripts/rosnodeList.sh b/web_interface/html/bashscripts/rosnodeList.sh
new file mode 100755
index 0000000000000000000000000000000000000000..bec3e55d01fa9712e20548db963a00ec5c828e9a
--- /dev/null
+++ b/web_interface/html/bashscripts/rosnodeList.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+source /opt/ros/melodic/setup.bash
+rosnode list
\ No newline at end of file
diff --git a/web_interface/html/bashscripts/updatewebinterface.sh b/web_interface/html/bashscripts/updatewebinterface.sh
new file mode 100755
index 0000000000000000000000000000000000000000..3c82c1028c0936ca734a2ead8a8cb3dfaa8bb04c
--- /dev/null
+++ b/web_interface/html/bashscripts/updatewebinterface.sh
@@ -0,0 +1,133 @@
+#!/bin/bash
+#
+# Inform the user
+echo "Now starting the \"updatewebinterface.sh\" shell script"
+echo ""
+#
+# Put the web home as a variable
+wwwhome="/var/www/html"
+#
+# Put the dfall home as a variable
+dfallhome="/home/www-share/dfall/dfall-system/web_interface/html"
+#
+# Inform the user of the directories
+echo "Using the following directories:"
+echo ">> wwwhome = $wwwhome"
+echo ">> dfallhome = $dfallhome"
+echo ""
+#
+# Change directory to the web home
+cd $wwwhome
+echo "Changed directory to $wwwhome"
+echo ""
+#
+#
+#
+# REMOVE the contents of the js folder
+rm -v js/*.js
+echo "Removed the \"*.js\" contents of the \"js\" folder"
+rm -v js/*
+echo "Removed any other contents of the \"js\" folder"
+rmdir -v js
+echo "Removed the \"js\" folder"
+echo ""
+#
+# Make the javacript folder again
+mkdir -v js
+echo "Remade the \"js\" folder"
+# Copy the javscripts files
+cp -v $dfallhome"/js/"*.js $wwwhome"/js/"
+echo "Copied across the \"js\" folder contents"
+echo ""
+#
+#
+#
+# REMOVE the contents of the img folder
+rm -v img/*.*
+echo "Removed the \"*.*\" contents of the \"img\" folder"
+rm -v img/*
+echo "Removed any other contents of the \"img\" folder"
+rmdir -v img
+echo "Removed the \"img\" folder"
+echo ""
+#
+# Make the img folder again
+mkdir -v img
+echo "Remade the \"img\" folder"
+# Copy the img files
+cp -v $dfallhome"/img/"*.* $wwwhome"/img/"
+echo "Copied across the \"img\" folder contents"
+echo ""
+#
+#
+#
+# REMOVE the contents of the css folder
+rm -v css/*.css
+echo "Removed the \"*.css\" contents of the \"css\" folder"
+rm -v css/*
+echo "Removed any other contents of the \"css\" folder"
+rmdir -v css
+echo "Removed the \"css\" folder"
+echo ""
+#
+# Make the img folder again
+mkdir -v css
+echo "Remade the \"css\" folder"
+# Copy the img files
+cp -v $dfallhome"/css/"*.css $wwwhome"/css/"
+echo "Copied across the \"css\" folder contents"
+echo ""
+#
+#
+#
+# REMOVE the contents of the bashscripts folder
+rm -v bashscripts/*.sh
+echo "Removed all the contents of the \"bashscripts\" folder"
+rmdir -v bashscripts
+echo "Removed the \"bashscripts\" folder"
+echo ""
+#
+# Make the img folder again
+mkdir -v bashscripts
+echo "Remade the \"bashscripts\" folder"
+# Copy the bashscript files
+cp -v $dfallhome"/bashscripts/"*.sh $wwwhome"/bashscripts/"
+echo "Copied across the \"bashscripts\" folder contents"
+echo ""
+#
+#
+#
+# Remove the "uploads" folder, if it exists
+rm -v uploads/*
+echo "Removed all contents of the \"uploads\" folder"
+rmdir -v uploads
+echo "Removed the \"uploads\" folder"
+echo ""
+#
+# Make the uploads folder again
+mkdir -v uploads
+echo "Remade the \"uploads\" folder"
+echo ""
+#
+#
+#
+# REMOVE the contents of the wwwhome folder
+rm -v *.php
+rm -v *.html
+rm -v *.*
+echo "Removed all the contents of the \"wwwhome\" folder"
+echo ""
+#
+# Copy the html files
+cp -v $dfallhome"/"*.html $wwwhome"/"
+echo "Copied across the \"*.html\" files"
+echo ""
+# Copy the php files
+cp -v $dfallhome"/"*.php $wwwhome"/"
+echo "Copied across the \"*.php\" files"
+echo ""
+#
+#
+#
+# Add write access for the group to all files
+#chmod -R g+rw $wwwhome
diff --git a/web_interface/html/callBashScript.php b/web_interface/html/callBashScript.php
new file mode 100644
index 0000000000000000000000000000000000000000..c3b7401da1783e312ee76e0c0114ebbea26619eb
--- /dev/null
+++ b/web_interface/html/callBashScript.php
@@ -0,0 +1,102 @@
+<?php
+ // Get the bash script name
+ $scriptname = $_GET['scriptname'];
+
+ // ONLY EXECUTE NAMES WITH AN EXACT MATHC
+
+
+ // For the GIT tab
+ if ($scriptname == "gitStatus") {
+ $temp = shell_exec("./bashscripts/gitStatus.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ elseif ($scriptname == "gitPull") {
+ $temp = shell_exec("./bashscripts/gitPull.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ elseif ($scriptname == "gitDiff") {
+ $temp = shell_exec("./bashscripts/gitDiff.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ elseif ($scriptname == "gitCheckoutAll") {
+ $temp = shell_exec("./bashscripts/gitCheckoutAll.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ elseif ($scriptname == "gitCheckoutMaster") {
+ $temp = shell_exec("./bashscripts/gitCheckoutMaster.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ elseif ($scriptname == "catkin_make") {
+ $temp = shell_exec("./bashscripts/catkin_make.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ //
+ // For the LAUNCH tab
+ elseif ($scriptname == "checkForRosMaster") {
+ $output = shell_exec("./bashscripts/checkForRosMaster.sh");
+ }
+ //
+ // "launch Ros Master" is handled separately
+ // because it requires an input argument
+ //
+ elseif ($scriptname == "killRosMaster") {
+ $output = shell_exec("./bashscripts/killRosMaster.sh");
+ }
+ elseif ($scriptname == "checkForRosAgent") {
+ $output = shell_exec("./bashscripts/checkForRosAgent.sh");
+ }
+ //
+ // "launch Ros Agent" is handled separately
+ // because it requires an input argument
+ //
+ elseif ($scriptname == "killRosAgent") {
+ $output = shell_exec("./bashscripts/killRosAgent.sh");
+ }
+ elseif ($scriptname == "rosnodeList") {
+ $temp = shell_exec("./bashscripts/rosnodeList.sh");
+ $output = "<pre>$temp</pre>";
+ }
+ //
+ // For the CONTROL tab:
+ // { RADIO CONNECTION, TAKE-OFF, LAND, MOTORS-OFF, LOAD CONTROLLER }
+ elseif ($scriptname == "rosConnect") {
+ $output = shell_exec("./bashscripts/rosCrazyRadioCommand_forAgent.sh 0");
+ }
+ elseif ($scriptname == "rosDisconnect") {
+ $output = shell_exec("./bashscripts/rosCrazyRadioCommand_forAgent.sh 1");
+ }
+ elseif ($scriptname == "rosTakeoff") {
+ $output = shell_exec("./bashscripts/rosFlyingStateCommand_forAgent.sh 11");
+ }
+ elseif ($scriptname == "rosLand") {
+ $output = shell_exec("./bashscripts/rosFlyingStateCommand_forAgent.sh 12");
+ }
+ elseif ($scriptname == "rosMotorsoff") {
+ $output = shell_exec("./bashscripts/rosFlyingStateCommand_forAgent.sh 13");
+ }
+ elseif ($scriptname == "rosEnabledefault") {
+ $output = shell_exec("./bashscripts/rosFlyingStateCommand_forAgent.sh 1");
+ }
+ elseif ($scriptname == "rosEnablestudent") {
+ $output = shell_exec("./bashscripts/rosFlyingStateCommand_forAgent.sh 3");
+ }
+ // For the CONTROL tab:
+ // GET SETPOINT
+ elseif ($scriptname == "rosGetSetpointDefault") {
+ $output = shell_exec("./bashscripts/rosGetCurrentSetpoint_forAgent.sh default");
+ }
+ elseif ($scriptname == "rosGetSetpointStudent") {
+ $output = shell_exec("./bashscripts/rosGetCurrentSetpoint_forAgent.sh student");
+ }
+ // For the CONTROL tab:
+ // LOAD YAML PARAMETERS
+ elseif ($scriptname == "rosLoadYamlDefault") {
+ $output = shell_exec("./bashscripts/rosLoadYaml_forAgent.sh default");
+ }
+ elseif ($scriptname == "rosLoadYamlStudent") {
+ $output = shell_exec("./bashscripts/rosLoadYaml_forAgent.sh student");
+ }
+
+
+ echo "$output";
+?>
diff --git a/web_interface/html/callBashScript_roslaunch.php b/web_interface/html/callBashScript_roslaunch.php
new file mode 100644
index 0000000000000000000000000000000000000000..bca63740adedd618bd5286582927aafabd45160d
--- /dev/null
+++ b/web_interface/html/callBashScript_roslaunch.php
@@ -0,0 +1,60 @@
+<?php
+ // GET THE BASH SCRIPT NAME
+ $scriptname = $_GET['scriptname'];
+
+ // ONLY EXECUTE "SCRIPT NAMES" WITH AN EXACT MATCH
+ //
+ // > For the MASTER
+ if ($scriptname == "master")
+ {
+ // GET THE VALUES OF THE EMULATE MOCAP FLAG
+ // > This will be a string
+ $emulate_mocap = strtolower( $_GET['emulatemocap'] );
+ // Check that the new setpoint values are numerical
+ if (in_array($emulate_mocap, array("true", "1", "yes"), true))
+ {
+ $emulate_mocap = "true";
+ }
+ elseif (in_array($emulate_mocap, array("false", "0", "no"), true))
+ {
+ $emulate_mocap = "false";
+ }
+ else
+ {
+ echo "emulate mocap flag = $emulate_mocap, is not a boolean value.";
+ exit();
+ }
+ // Call the bash script for launching the master
+ $output = shell_exec("./bashscripts/launchRosMaster.sh $emulate_mocap");
+ }
+ //
+ // > For the AGENT
+ elseif ($scriptname == "agent")
+ {
+ // GET THE VALUES OF THE EMULATE MOCAP FLAG
+ // > This will be a string
+ $emulate_crazyradio = strtolower( $_GET['emulatecrazyradio'] );
+ // Check that the new setpoint values are numerical
+ if (in_array($emulate_crazyradio, array("true", "1", "yes"), true))
+ {
+ $emulate_crazyradio = "true";
+ }
+ elseif (in_array($emulate_crazyradio, array("false", "0", "no"), true))
+ {
+ $emulate_crazyradio = "false";
+ }
+ else
+ {
+ echo "emulate crazyradio flag = $emulate_crazyradio, is not a boolean value.";
+ exit();
+ }
+ // Call the bash script for launching the agent
+ $output = shell_exec("./bashscripts/launchRosAgent.sh $emulate_crazyradio");
+ }
+ else
+ {
+ $output = "launch name = $scriptname is not a valid option";
+ }
+
+ echo "$output";
+?>
diff --git a/web_interface/html/callBashScript_setNewSetpoint.php b/web_interface/html/callBashScript_setNewSetpoint.php
new file mode 100644
index 0000000000000000000000000000000000000000..2e44ae0c6df7e69af0ffe15b82028f21c14bba6c
--- /dev/null
+++ b/web_interface/html/callBashScript_setNewSetpoint.php
@@ -0,0 +1,47 @@
+<?php
+ // GET THE BASH SCRIPT NAME
+ $scriptname = $_GET['scriptname'];
+
+ // GET THE (x,y,z,yaw) VALUES OF THE NEW SETPOINT
+ // > These will be strings
+ $x_new = $_GET['x'];
+ $y_new = $_GET['y'];
+ $z_new = $_GET['z'];
+ $yaw_new = $_GET['yaw'];
+
+
+ // Check that the new setpoint values are numerical
+ if ( ! (is_numeric($x_new)) )
+ {
+ echo "x = $x_new, is not a numeric value.";
+ exit();
+ }
+ if ( ! (is_numeric($y_new)) )
+ {
+ echo "y = $y_new, is not a numeric value.";
+ exit();
+ }
+ if ( ! (is_numeric($z_new)) )
+ {
+ echo "z = $z_new, is not a numeric value.";
+ exit();
+ }
+ if ( ! (is_numeric($yaw_new)) )
+ {
+ echo "yaw = $yaw_new, is not a numeric value.";
+ exit();
+ }
+
+ // ONLY EXECUTE "SCRIPT NAMES" WITH AN EXACT MATCH
+ // For the CONTROL tab:
+ // GET SETPOINT
+ if ($scriptname == "rosSetSetpointDefault") {
+ $output = shell_exec("./bashscripts/rosSetNewSetpoint_forAgent.sh default $x_new $y_new $z_new $yaw_new");
+ }
+ elseif ($scriptname == "rosSetSetpointStudent") {
+ $output = shell_exec("./bashscripts/rosSetNewSetpoint_forAgent.sh student $x_new $y_new $z_new $yaw_new");
+ }
+
+
+ echo "$output";
+?>
diff --git a/web_interface/html/css/body.css b/web_interface/html/css/body.css
new file mode 100644
index 0000000000000000000000000000000000000000..021b02f565784c7234f6f53c110dcd96715dcfca
--- /dev/null
+++ b/web_interface/html/css/body.css
@@ -0,0 +1,112 @@
+/* RESTRICT PAGE WIDTH TO A MAXIMUM */
+.body-container
+{
+ position: relative;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ margin-right: 0px;
+ max-width: 800px;
+ padding: 0px;
+}
+
+
+/* MAIN STYLE */
+.body-container > main
+{
+ color: black;
+ font-family: 'Lato', Arial, sans-serif;
+ font-weight: 400;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ margin-right: 0px;
+ padding-top: 0.2em;
+ padding-bottom: 1em;
+ padding-left: 30px;
+ padding-right: 30px;
+ text-align: left;
+ background: rgba(0,0,0,0.0);
+}
+
+.body-container > main a
+{
+ color: #a5d0ff;
+ text-decoration: none;
+ outline: none;
+}
+
+.body-container > main a:hover
+{
+ color: #60abff;
+}
+
+.body-container > main h1
+{
+ font-size: 3.2em;
+ line-height: 1.3;
+ margin: 0px;
+ font-weight: 350;
+}
+
+.body-container > main h2
+{
+ font-size: 1.4em;
+ line-height: 1.3;
+ margin: 0px;
+ padding-top: 0.1em;
+ padding-bottom: 0.2em;
+ padding-left: 20px;
+ padding-right: 20px;
+ font-weight: 400;
+}
+
+.body-container > main span
+{
+ display: block;
+ font-size: 55%;
+ color: black;
+ padding: 0 0 0.6em 0.1em;
+}
+
+.hr-basic
+{
+ background-color: white;
+ width: 100%;
+ height: 2px;
+ border: 0px solid black;
+}
+
+.hr-basic.navy
+{
+ background-color: #34495E;
+}
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+/* IMPORTANT NOTES:
+ The <meta> tag in the <head> of the page is
+ very important for determining the behaviour
+ this media trigger, i.e., the tag:
+ <meta name="viewport" content="width=device-width, initial-scale=1.0">
+ For example, with this tag the iPhone SE has
+ a "max-width"=320 and the same font sizes work
+ (approximately) on both a computer/laptop
+ browser and a smartphone browser. Whereas,
+ without this tag the iPhone SE has a
+ "mx-width"=768 and the sizes are much too small
+ when viewed on a smartphone.
+*/
+@media screen and (max-width: 420px)
+{
+ .body-container > main
+ {
+ padding-top: 0.2em;
+ padding-bottom: 1em;
+ padding-left: 0px;
+ padding-right: 0px;
+ font-size: 100%;
+ text-align: left;
+ //background: rgba(0,255,0,1.0);
+ }
+}
diff --git a/web_interface/html/css/buttons.css b/web_interface/html/css/buttons.css
new file mode 100644
index 0000000000000000000000000000000000000000..c69685cb5a59749b9a32dca80ac5773b99cd3050
--- /dev/null
+++ b/web_interface/html/css/buttons.css
@@ -0,0 +1,344 @@
+.button-push
+{
+ position: relative;
+ background-color: #ddd;
+ border: 1px solid #bbb;
+ border-radius: 6px;
+ color: #555;
+ text-shadow: 0 1px 0 rgba(0,0,0,.2);
+ margin: 0px;
+ padding-left: 30px;
+ padding-right: 30px;
+ padding-top: 15px;
+ padding-bottom: 15px;
+ text-align: center;
+ text-decoration: none;
+ display: inline-block;
+ font-size: 1.4em;
+ cursor: grabbing;
+
+ overflow: hidden;
+}
+
+
+.button-push:hover
+{
+ background-color: #eee;
+}
+
+.button-push:active
+{
+ background: #e9e9e9;
+ top: 1px;
+ text-shadow: none;
+ box-shadow: 0 1px 1px rgba(0, 0, 0, .3) inset;
+
+
+}
+
+.button-push > .div-for-button-highlight-on-touchscreen
+{
+ content: "test";
+ position: absolute;
+ top: 0;
+ left: 0;
+ height: 100%;
+ width: 100%;
+ padding-bottom: 0;
+ border-radius: 0px;
+ background-color: #000;
+ transform: scale(1.0);
+ opacity: 0.0;
+}
+
+
+.div-for-button-highlight-on-touchscreen.red{ background-color: #000;}
+.div-for-button-highlight-on-touchscreen.green{ background-color: #000;}
+.div-for-button-highlight-on-touchscreen.blue{ background-color: #000;}
+.div-for-button-highlight-on-touchscreen.navy{ background-color: #eee;}
+
+
+/* THE FOLLOWING ACTUALLY ADDS THE ANIMATION */
+/*
+.button-push:focus:not(:active) > .div-for-button-highlight-on-touchscreen
+{
+ animation: click-animation 0.6s linear;
+ animation-fill-mode: backwards;
+}
+*/
+
+@keyframes click-animation {
+ 0% {
+ opacity: 0.2;
+ transform: scale(1.0);
+ }
+ 50% {
+ opacity: 0.50;
+ transform: scale(1.0);
+ }
+ 70% {
+ opacity: 0.5;
+ transform: scale(1.0);
+ }
+ 100% {
+ opacity: 0;
+ transform: scale(1.0);
+ }
+}
+
+
+
+
+
+
+
+
+
+/* SET THE DISPLAY STYLE TO BLOCK */
+.button-push.fullwidth
+{
+ display: block;
+ width: 100%;
+}
+
+/* SET THE FONT TO A FIXED WIDTH FONT */
+.button-push.monospace
+{
+ font-family: monospace;
+}
+
+/* SET THE TOP AND BOTTOM SPACING TO BE SMALL */
+.button-push.short
+{
+ padding-top: 3px;
+ padding-bottom: 3px;
+}
+.button-push.short5
+{
+ padding-top: 5px;
+ padding-bottom: 5px;
+}
+.button-push.short10
+{
+ padding-top: 5px;
+ padding-bottom: 5px;
+}
+
+/* SET THE LEFT AND RIGHT SPACING TO BE SMALL */
+.button-push.slim
+{
+ padding-left: 10px;
+ padding-right: 10px;
+}
+
+/* SET SIZING FOR THE MOTORS-OFF BUTTON */
+.button-push.motorsoff
+{
+ display: block;
+ width: 100%;
+ text-align: center;
+ font-weight: 600;
+}
+
+/* SET SIZING FOR THE MOTORS-OFF BUTTON */
+.button-push.info
+{
+ display: inline
+ width: 100%;
+ text-align: center;
+ font-weight: bolder;
+ font-size: 24px!important;
+ padding: 8px 20px;
+ vertical-align: middle;
+ overflow: hidden;
+ text-decoration: none;
+ white-space:nowrap;
+ border-radius: 50%;
+}
+
+
+
+
+
+/* SET THE APPEARANCE FOR WHEN DISABLED */
+.button-push[disabled], .button-push[disabled]:hover, .button-push[disabled]:active
+{
+ border-color: #eaeaea;
+ background: #fafafa;
+ cursor: default;
+ position: static;
+ color: #999;
+ /* Usually, !important should be avoided but here it's really needed :) */
+ /*
+ -moz-box-shadow: none !important;
+ -webkit-box-shadow: none !important;
+ box-shadow: none !important;
+ text-shadow: none !important;
+ */
+
+}
+
+
+/* SET COLOURS FOR RED BUTTON */
+.button-push.red
+{
+ background-color: #f44336;
+ border-color: #c43c35;
+ color: white;
+}
+
+.button-push.red:hover
+{
+ background-color: #ee5f5b;
+}
+
+.button-push.red:active
+{
+ background: #c43c35;
+}
+
+.red[disabled], .red[disabled]:hover, .red[disabled]:active
+{
+ border-color: #C43C35;
+ background: #C43C35;
+ color: #FFD3D3;
+}
+
+
+/* SET COLOURS FOR GREEN BUTTON */
+.button-push.green
+{
+ background-color: #57a957;
+ border-color: #57a957;
+ color: white;
+}
+
+.button-push.green:hover
+{
+ background-color: #62c462;
+}
+
+.button-push.green:active
+{
+ background: #57a957;
+}
+
+.green[disabled], .green[disabled]:hover, .green[disabled]:active
+{
+ border-color: #57A957;
+ background: #57A957;
+ color: #D2FFD2;
+}
+
+
+/* SET COLOURS FOR BLUE BUTTON */
+.button-push.blue
+{
+ background-color: #269CE9;
+ border-color: #269CE9;
+ color: white;
+}
+
+.button-push.blue:hover
+{
+ background-color: #70B9E8;
+}
+
+.button-push.blue:active
+{
+ background: #269CE9;
+}
+
+.blue[disabled], .blue[disabled]:hover, .blue[disabled]:active
+{
+ border-color: #269CE9;
+ background: #269CE9;
+ color: #93D5FF;
+}
+
+/* SET COLOURS FOR NAVY BUTTON */
+.button-push.navy
+{
+ background-color: #34495E;
+ border-color: #34495E;
+ color: white;
+}
+
+.button-push.navy:hover
+{
+ background-color: #748DA6;
+}
+
+.button-push.navy:active
+{
+ background: #34495E;
+}
+
+.navy[disabled], .navy[disabled]:hover, .navy[disabled]:active
+{
+ border-color: #34495E;
+ background: #34495E;
+ color: #93D5FF;
+}
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ .button-push
+ {
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ font-size: 1.2em;
+ }
+
+ .button-push.info
+ {
+ font-size: 28px!important;
+ padding: 8px 20px;
+ }
+
+ /* MAKE THE HOVER COLOUR THE SAME AS THE NORMAL COLOUR */
+ .button-push:hover{ background-color: #ddd; }
+ .button-push.red:hover{ background-color: #f44336;}
+ .button-push.green:hover{ background-color: #57a957;}
+ .button-push.blue:hover{ background-color: #269CE9;}
+ .button-push.navy:hover{ background-color: #34495E;}
+
+ /* SET THE DISABLED HOVER COLOUR */
+ /* NEEDED AGAIN DESPITE BEING DONE ABOVE */
+ .red[disabled]:hover
+ {
+ border-color: #C43C35;
+ background: #C43C35;
+ color: #FFD3D3;
+ }
+ .green[disabled]:hover
+ {
+ border-color: #57A957;
+ background: #57A957;
+ color: #D2FFD2;
+ }
+ .blue[disabled]:hover
+ {
+ border-color: #269CE9;
+ background: #269CE9;
+ color: #93D5FF;
+ }
+ .navy[disabled]:hover
+ {
+ border-color: #34495E;
+ background: #34495E;
+ color: #93D5FF;
+ }
+
+ .button-push:hover:not(:active) > .div-for-button-highlight-on-touchscreen
+ {
+ animation: click-animation 0.7s ease-out;
+ }
+
+}
diff --git a/web_interface/html/css/buttons_from_some_website.css b/web_interface/html/css/buttons_from_some_website.css
new file mode 100644
index 0000000000000000000000000000000000000000..54e8dbe95480bf955682afaa6778da9559ec3aaa
--- /dev/null
+++ b/web_interface/html/css/buttons_from_some_website.css
@@ -0,0 +1,250 @@
+/*
+ TAKEN FROM:
+ https://codepen.io/ben_jammin/pen/syaCq
+*/
+
+/*
+body{
+ background:url(http://subtlepatterns.subtlepatterns.netdna-cdn.com/patterns/wild_oliva.png);
+}
+*/
+
+.centered {
+ margin:50px auto;
+ text-align:center;
+}
+
+.button::-moz-focus-inner{
+ border: 0;
+ padding: 0;
+}
+
+.button{
+ display: inline-block;
+ *display: inline;
+ zoom: 1;
+ padding: 6px 20px;
+ margin: 0;
+ cursor: pointer;
+ border: 1px solid #bbb;
+ overflow: visible;
+ font: bold 13px arial, helvetica, sans-serif;
+ text-decoration: none;
+ white-space: nowrap;
+ color: #555;
+
+ background-color: #ddd;
+ background-image: -webkit-gradient(linear, left top, left bottom, from(rgba(255,255,255,1)), to(rgba(255,255,255,0)));
+ background-image: -webkit-linear-gradient(top, rgba(255,255,255,1), rgba(255,255,255,0));
+ background-image: -moz-linear-gradient(top, rgba(255,255,255,1), rgba(255,255,255,0));
+ background-image: -ms-linear-gradient(top, rgba(255,255,255,1), rgba(255,255,255,0));
+ background-image: -o-linear-gradient(top, rgba(255,255,255,1), rgba(255,255,255,0));
+ background-image: linear-gradient(top, rgba(255,255,255,1), rgba(255,255,255,0));
+
+ -webkit-transition: background-color .2s ease-out;
+ -moz-transition: background-color .2s ease-out;
+ -ms-transition: background-color .2s ease-out;
+ -o-transition: background-color .2s ease-out;
+ transition: background-color .2s ease-out;
+ background-clip: padding-box; /* Fix bleeding */
+ -moz-border-radius: 3px;
+ -webkit-border-radius: 3px;
+ border-radius: 3px;
+ -moz-box-shadow: 0 1px 0 rgba(0, 0, 0, .3), 0 2px 2px -1px rgba(0, 0, 0, .5), 0 1px 0 rgba(255, 255, 255, .3) inset;
+ -webkit-box-shadow: 0 1px 0 rgba(0, 0, 0, .3), 0 2px 2px -1px rgba(0, 0, 0, .5), 0 1px 0 rgba(255, 255, 255, .3) inset;
+ box-shadow: 0 1px 0 rgba(0, 0, 0, .3), 0 2px 2px -1px rgba(0, 0, 0, .5), 0 1px 0 rgba(255, 255, 255, .3) inset;
+ text-shadow: 0 1px 0 rgba(255,255,255, .9);
+
+ -webkit-touch-callout: none;
+ -webkit-user-select: none;
+ -khtml-user-select: none;
+ -moz-user-select: none;
+ -ms-user-select: none;
+ user-select: none;
+}
+
+.button:hover{
+ background-color: #eee;
+ color: #555;
+}
+
+.button:active{
+ background: #e9e9e9;
+ position: relative;
+ top: 1px;
+ text-shadow: none;
+ -moz-box-shadow: 0 1px 1px rgba(0, 0, 0, .3) inset;
+ -webkit-box-shadow: 0 1px 1px rgba(0, 0, 0, .3) inset;
+ box-shadow: 0 1px 1px rgba(0, 0, 0, .3) inset;
+}
+
+.button[disabled], .button[disabled]:hover, .button[disabled]:active{
+ border-color: #eaeaea;
+ background: #fafafa;
+ cursor: default;
+ position: static;
+ color: #999;
+ /* Usually, !important should be avoided but here it's really needed :) */
+ -moz-box-shadow: none !important;
+ -webkit-box-shadow: none !important;
+ box-shadow: none !important;
+ text-shadow: none !important;
+}
+
+/* Smaller buttons styles */
+
+.button.small{
+ padding: 4px 12px;
+}
+
+/* Larger buttons styles */
+
+.button.large{
+ padding: 12px 30px;
+ text-transform: uppercase;
+}
+
+.button.large:active{
+ top: 2px;
+}
+
+/* Colored buttons styles */
+
+.button.green, .button.red, .button.blue {
+ color: #fff;
+ text-shadow: 0 1px 0 rgba(0,0,0,.2);
+
+ background-image: -webkit-gradient(linear, left top, left bottom, from(rgba(255,255,255,.3)), to(rgba(255,255,255,0)));
+ background-image: -webkit-linear-gradient(top, rgba(255,255,255,.3), rgba(255,255,255,0));
+ background-image: -moz-linear-gradient(top, rgba(255,255,255,.3), rgba(255,255,255,0));
+ background-image: -ms-linear-gradient(top, rgba(255,255,255,.3), rgba(255,255,255,0));
+ background-image: -o-linear-gradient(top, rgba(255,255,255,.3), rgba(255,255,255,0));
+ background-image: linear-gradient(top, rgba(255,255,255,.3), rgba(255,255,255,0));
+}
+
+/* */
+
+.button.green{
+ background-color: #57a957;
+ border-color: #57a957;
+}
+
+.button.green:hover{
+ background-color: #62c462;
+}
+
+.button.green:active{
+ background: #57a957;
+}
+
+/* */
+
+.button.red{
+ background-color: #ca3535;
+ border-color: #c43c35;
+}
+
+.button.red:hover{
+ background-color: #ee5f5b;
+}
+
+.button.red:active{
+ background: #c43c35;
+}
+
+/* */
+
+.button.blue{
+ background-color: #269CE9;
+ border-color: #269CE9;
+}
+
+.button.blue:hover{
+ background-color: #70B9E8;
+}
+
+.button.blue:active{
+ background: #269CE9;
+}
+
+/* */
+
+.green[disabled], .green[disabled]:hover, .green[disabled]:active{
+ border-color: #57A957;
+ background: #57A957;
+ color: #D2FFD2;
+}
+
+.red[disabled], .red[disabled]:hover, .red[disabled]:active{
+ border-color: #C43C35;
+ background: #C43C35;
+ color: #FFD3D3;
+}
+
+.blue[disabled], .blue[disabled]:hover, .blue[disabled]:active{
+ border-color: #269CE9;
+ background: #269CE9;
+ color: #93D5FF;
+}
+
+/* Group buttons */
+
+.button-group,
+.button-group li{
+ display: inline-block;
+ *display: inline;
+ zoom: 1;
+}
+
+.button-group{
+ font-size: 0; /* Inline block elements gap - fix */
+ margin: 0;
+ padding: 0;
+ background: rgba(0, 0, 0, .1);
+ border-bottom: 1px solid rgba(0, 0, 0, .1);
+ padding: 7px;
+ -moz-border-radius: 7px;
+ -webkit-border-radius: 7px;
+ border-radius: 7px;
+}
+
+.button-group li{
+ margin-right: -1px; /* Overlap each right button border */
+}
+
+.button-group .button{
+ font-size: 13px; /* Set the font size, different from inherited 0 */
+ -moz-border-radius: 0;
+ -webkit-border-radius: 0;
+ border-radius: 0;
+}
+
+.button-group .button:active{
+ -moz-box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, 5px 0 5px -3px rgba(0, 0, 0, .2) inset, -5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+ -webkit-box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, 5px 0 5px -3px rgba(0, 0, 0, .2) inset, -5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+ box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, 5px 0 5px -3px rgba(0, 0, 0, .2) inset, -5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+}
+
+.button-group li:first-child .button{
+ -moz-border-radius: 3px 0 0 3px;
+ -webkit-border-radius: 3px 0 0 3px;
+ border-radius: 3px 0 0 3px;
+}
+
+.button-group li:first-child .button:active{
+ -moz-box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, -5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+ -webkit-box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, -5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+ box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, -5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+}
+
+.button-group li:last-child .button{
+ -moz-border-radius: 0 3px 3px 0;
+ -webkit-border-radius: 0 3px 3px 0;
+ border-radius: 0 3px 3px 0;
+}
+
+.button-group li:last-child .button:active{
+ -moz-box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, 5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+ -webkit-box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, 5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+ box-shadow: 0 0 1px rgba(0, 0, 0, .2) inset, 5px 0 5px -3px rgba(0, 0, 0, .2) inset;
+}
\ No newline at end of file
diff --git a/web_interface/html/css/debug_values_table.css b/web_interface/html/css/debug_values_table.css
new file mode 100644
index 0000000000000000000000000000000000000000..81e5fef1c21631861d2121933428323b406b80c5
--- /dev/null
+++ b/web_interface/html/css/debug_values_table.css
@@ -0,0 +1,91 @@
+.debug-values-table
+{
+ margin-left: auto;
+ margin-right: auto;
+ padding: 0;
+ border-collapse: collapse;
+ table-layout: fixed;
+ //border: 1px solid black;
+ //border-left: 2px solid black;
+}
+
+.debug-values-column-title-cell
+{
+ margin: 0;
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+ border: 1px solid black;
+}
+
+.debug-values-row-title-cell
+{
+ margin: 0;
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+ border: 1px solid black;
+}
+
+.debug-values-data-cell
+{
+ margin: 0;
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: normal;
+ font-family: monospace;
+ text-align: right;
+ border: 1px solid black;
+}
+
+.debug-values-caption-below
+{
+ margin: 0;
+ padding-left: 5px;
+ padding-right: 5px;
+ padding-top: 5px;
+ padding-bottom: 2px;
+ font-size: 1.2em;
+ font-weight: normal;
+ text-align: left;
+ //border: 1px solid black;
+}
+
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ .debug-values-column-title-cell
+ {
+ font-size: 1.2em;
+ }
+
+ .debug-values-row-title-cell
+ {
+ font-size: 1.2em;
+ }
+
+ .debug-values-data-cell
+ {
+ font-size: 1.2em;
+ }
+
+ .debug-values-caption-below
+ {
+ font-size: 1.0em;
+ }
+}
diff --git a/web_interface/html/css/default.css b/web_interface/html/css/default.css
new file mode 100644
index 0000000000000000000000000000000000000000..036f5f4219b938d58b38f083aef8b0d4da57b95b
--- /dev/null
+++ b/web_interface/html/css/default.css
@@ -0,0 +1,8 @@
+/* IMPORT THE FONT */
+@import url(https://fonts.googleapis.com/css?family=Lato:300,400,700);
+
+/* SET THE BACKGROUND COLOUR */
+body
+{
+ background: #34495e;
+}
\ No newline at end of file
diff --git a/web_interface/html/css/fileinput.css b/web_interface/html/css/fileinput.css
new file mode 100644
index 0000000000000000000000000000000000000000..43927246f37ed7b96e8075bf406dc560e5d6fdb6
--- /dev/null
+++ b/web_interface/html/css/fileinput.css
@@ -0,0 +1,62 @@
+.file-input-label
+{
+ margin: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ text-align: left;
+ text-decoration: none;
+ display: inline-block;
+ font-size: 1.4em;
+}
+
+.file-input
+{
+ margin: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ text-align: left;
+ text-decoration: none;
+ display: inline-block;
+ font-size: 1.0em;
+}
+
+.file-upload-status-div
+{
+ margin: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ text-align: left;
+ text-decoration: none;
+ display: inline-block;
+ font-size: 1.0em;
+}
+
+.file-input-details-div
+{
+ margin: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ text-align: left;
+ text-decoration: none;
+ display: inline-block;
+ font-size: 1.0em;
+}
+
+
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+
+}
diff --git a/web_interface/html/css/layout.css b/web_interface/html/css/layout.css
new file mode 100644
index 0000000000000000000000000000000000000000..ec5d25d4b681939373255158aef2662a3572c310
--- /dev/null
+++ b/web_interface/html/css/layout.css
@@ -0,0 +1,356 @@
+.full-window-fixed
+{
+ position: fixed;
+ top: 0;
+ bottom: 0;
+ left: 0;
+ right: 0;
+ background: #34495E;
+ width: 100%;
+ margin: 0;
+ padding: 0;
+}
+
+.max-width-full-heigth-fixed
+{
+ color: #fff;
+ font-family: 'Lato', Arial, sans-serif;
+ position: fixed;
+ top: 0;
+ bottom: 0;
+ left: 0px;
+ right: 0px;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: auto;
+ margin-right: auto;
+ max-width: 800px;
+ padding: 0px;
+ background: rgba(0,0,0,0.10);
+}
+
+.top-bar-container
+{
+ color: #fff;
+ font-family: 'Lato', Arial, sans-serif;
+ position: relative;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ margin-right: 0px;
+ max-width: 800px;
+ padding: 0px;
+ background: none;
+}
+
+.top-bar-buttons-table
+{
+ position: relative;
+ width: 100%;
+ border: none;
+ border-spacing: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ padding-left: 20px;
+ padding-right: 20px;
+ background: none;
+}
+
+.top-bar-motorsoff-button-cell
+{
+ padding-right: 10px;
+}
+
+.top-bar-info-button-cell
+{
+ width: 20px;
+}
+
+.top-bar-title
+{
+ position: relative;
+ width: 100%;
+ text-align: center;
+ font-size: 2.0em;
+ font-weight: 600;
+ padding-top: 0px;
+ padding-bottom: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ background: none;
+ white-space: nowrap;
+ overflow-x: hidden;
+}
+.top-bar-title.padbelow
+{
+ padding-bottom: 10px;
+}
+
+.top-bar-subtitle
+{
+ position: relative;
+ width: 100%;
+ text-align: center;
+ font-size: 1.2em;
+ font-weight: 400;
+ padding-top: 0px;
+ padding-bottom: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ background: none;
+ white-space: nowrap;
+ overflow-x: hidden;
+}
+
+.top-bar-subtitle.padbelow
+{
+ padding-bottom: 10px;
+}
+
+.top-bar-status-icons
+{
+ position: relative;
+ width: 100%;
+ text-align: center;
+ font-size: 1.2em;
+ font-weight: 400;
+ padding-top: 0px;
+ padding-bottom: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ background: none;
+ white-space: nowrap;
+ overflow-x: hidden;
+}
+.top-bar-status-icons.padbelow
+{
+ padding-bottom: 5px;
+}
+
+
+.ros-table
+{
+ position: relative;
+ width: 100%;
+ table-layout: fixed;
+ border-spacing: 0px;
+ padding-top: 0px;
+ padding-bottom: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ background: none;
+ //border: 1px solid black;
+}
+
+.ros-button-cell
+{
+ width: 50%;
+ padding-top: 5px;
+ padding-bottom: 5px;
+ padding-left: 0px;
+ padding-right: 0px;
+ //border: 1px solid black;
+}
+
+.ros-info-cell
+{
+ padding-left: 10px;
+ padding-right: 10px;
+ text-align: left;
+ //border: 1px solid black;
+}
+
+
+
+.switch-cell
+{
+ padding-top: 5px;
+ padding-bottom: 5px;
+ padding-left: 0px;
+ padding-right: 0px;
+ text-align: left;
+ //border: 1px solid black;
+}
+
+.switch-cell.pad-right
+{
+ padding-right: 5px;
+}
+
+.switch-cell.pad-left
+{
+ padding-left: 5px;
+}
+
+
+
+.git-table
+{
+ position: relative;
+ width: 100%;
+ table-layout: fixed;
+ border-spacing: 0px;
+ margin: 0px;
+ padding-top: 0px;
+ padding-bottom: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ background: none;
+ //border: 1px solid black;
+}
+
+.git-button-cell
+{
+ width: 50%;
+ margin: 0px;
+ padding-top: 5px;
+ padding-bottom: 5px;
+ padding-left: 5px;
+ padding-right: 5px;
+ //border: 1px solid black;
+}
+
+.git-button-cell.left
+{
+ padding-left: 0px;
+}
+
+.git-button-cell.right
+{
+ padding-right: 0px;
+}
+
+
+.git-checkout-label
+{
+ color: #f44336;
+ font-family: monospace;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+ margin-top: 10px;
+}
+
+
+.terminal-label-table
+{
+ position: relative;
+ table-layout: fixed;
+ border-spacing: 0px;
+ margin-left: auto;
+ margin-right: auto;
+ margin-bottom: 3px;
+ padding: 0px;
+ background: none;
+ //border: 1px solid black;
+}
+
+.terminal-label-cell
+{
+ padding-right: 5px;
+}
+
+.terminal-label
+{
+ color: #34495E;
+ font-family: monospace;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+}
+
+.terminal-background
+{
+ position: relative;
+ width: 100%;
+ color: white;
+ background-color: #34495E;
+ margin: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ padding-left: 0px;
+ padding-right: 0px;
+ border-radius: 5px;
+ overflow-x: auto;
+}
+
+.terminal-contents
+{
+ position: relative;
+ color: white;
+ background-color: none;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 5px;
+ margin-right: 5px;
+ padding: 0px;
+}
+
+.terminal-highligh-paragraph
+{
+ color: white;
+ background-color: #269CE9;
+ text-align: center;
+}
+
+.centered-table
+{
+ position: relative;
+ //table-layout: fixed;
+ border-spacing: 0px;
+ padding-top: 0px;
+ padding-bottom: 0px;
+ padding-left: 0px;
+ padding-right: 0px;
+ margin-left: auto;
+ margin-right: auto;
+ background: none;
+ //border: 1px solid black;
+}
+
+.centered-table-button-cell
+{
+ margin: 0px;
+ padding-top: 5px;
+ padding-bottom: 5px;
+ padding-left: 5px;
+ padding-right: 5px;
+ //border: 1px solid black;
+}
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ .top-bar-title
+ {
+ font-size: 1.5em;
+ }
+
+ .top-bar-title.padbelow
+ {
+ padding-bottom: 5px;
+ }
+
+ .top-bar-subtitle
+ {
+ font-size: 1.0em;
+ }
+
+ .top-bar-subtitle.padbelow
+ {
+ padding-bottom: 5px;
+ }
+
+ .ros-button-cell
+ {
+ width: 60%;
+ }
+
+ .terminal-background
+ {
+ width: 86%;
+ margin-left: 7%;
+ }
+
+}
\ No newline at end of file
diff --git a/web_interface/html/css/measurement_setpoint_error_table.css b/web_interface/html/css/measurement_setpoint_error_table.css
new file mode 100644
index 0000000000000000000000000000000000000000..48f0e7fcb54af2ce368cdb126c0f83b95b59cc44
--- /dev/null
+++ b/web_interface/html/css/measurement_setpoint_error_table.css
@@ -0,0 +1,151 @@
+.setpoint-label-cell
+{
+ margin: 0;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+}
+
+.setpoint-increment-cell
+{
+ margin: 0;
+ padding-top: 5px;
+ padding-bottom: 5px;
+}
+
+.setpoint-input-field-cell
+{
+ margin: 0;
+ padding-top: 5px;
+ padding-bottom: 5px;
+}
+
+.setpoint-input-field
+{
+ width:7em;
+ margin: 0;
+ padding-top: 5px;
+ padding-bottom: 5px;
+ font-size: 1.4em;
+ font-family: monospace;
+ text-align: right;
+}
+
+
+
+
+
+.mse-table
+{
+ margin-left: auto;
+ margin-right: auto;
+ padding: 0;
+ border-collapse: collapse;
+ table-layout: fixed;
+ //border: 1px solid black;
+ //border-left: 2px solid black;
+}
+
+.mse-column-title-cell
+{
+ width: 28%;
+ margin: 0;
+ padding-left: 5px;
+ padding-right: 5px;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+ border: 1px solid black;
+}
+
+.mse-column-title-span-full-text
+{
+ display: inline;
+}
+
+.mse-column-title-span-short-text
+{
+ display: none;
+}
+
+.mse-row-title-cell
+{
+ margin: 0;
+ padding-left: 2px;
+ padding-right: 2px;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: bold;
+ text-align: center;
+ border: 1px solid black;
+}
+
+.mse-data-cell
+{
+ margin: 0;
+ padding-left: 5px;
+ padding-right: 5px;
+ padding-top: 2px;
+ padding-bottom: 2px;
+ font-size: 1.4em;
+ font-weight: normal;
+ font-family: monospace;
+ text-align: right;
+ border: 1px solid black;
+}
+
+.mse-caption-below
+{
+ margin: 0;
+ padding-left: 5px;
+ padding-right: 5px;
+ padding-top: 5px;
+ padding-bottom: 2px;
+ font-size: 1.2em;
+ font-weight: normal;
+ text-align: left;
+ //border: 1px solid black;
+}
+
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ .mse-column-title-cell
+ {
+ font-size: 1.2em;
+ }
+
+ .mse-row-title-cell
+ {
+ font-size: 1.2em;
+ }
+
+ .mse-column-title-span-full-text
+ {
+ display: none;
+ }
+
+ .mse-column-title-span-short-text
+ {
+ display: inline;
+ }
+
+ .mse-data-cell
+ {
+ font-size: 1.2em;
+ }
+
+ .mse-caption-below
+ {
+ font-size: 1.0em;
+ }
+}
diff --git a/web_interface/html/css/status_bar.css b/web_interface/html/css/status_bar.css
new file mode 100644
index 0000000000000000000000000000000000000000..936fc8f614551181e7ab24d9340f0797d8d64500
--- /dev/null
+++ b/web_interface/html/css/status_bar.css
@@ -0,0 +1,71 @@
+.status-icon-radio
+{
+ height: 40px;
+ width: 55px;
+ object-fit: contain;
+ text-align: center;
+ margin-left: 0px;
+ margin-right: 0px;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 0px;
+ padding-bottom: 0px;
+}
+
+.status-icon-battery
+{
+ height: 40px;
+ width: 26px;
+ object-fit: contain;
+ text-align: center;
+ margin-left: 0px;
+ margin-right: 0px;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 0px;
+ padding-bottom: 0px;
+}
+
+.status-icon-flying-state
+{
+ height: 40px;
+ width: 51px;
+ object-fit: contain;
+ text-align: center;
+ margin-left: 0px;
+ margin-right: 0px;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ padding-left: 10px;
+ padding-right: 10px;
+ padding-top: 0px;
+ padding-bottom: 0px;
+}
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ .status-icon-radio
+ {
+ padding-left: 5px;
+ padding-right: 5px;
+ }
+
+ .status-icon-battery
+ {
+ padding-left: 5px;
+ padding-right: 5px;
+ }
+
+ .status-icon-flying-state
+ {
+ padding-left: 5px;
+ padding-right: 5px;
+ }
+}
\ No newline at end of file
diff --git a/web_interface/html/css/switches.css b/web_interface/html/css/switches.css
new file mode 100644
index 0000000000000000000000000000000000000000..2fcdce35b8cce2d5a87c0c5b9a406510ca5ab798
--- /dev/null
+++ b/web_interface/html/css/switches.css
@@ -0,0 +1,505 @@
+/* =========================== */
+/* === PLAIN SWITCH === */
+/* =========================== */
+.on-off-switch-neighbouring-labelon-off-switch-neighbouring-label
+{
+ display: inline;
+ line-height: 90px;
+}
+
+.on-off-switch
+{
+ position: relative;
+ display: inline-block;
+}
+
+.on-off-switch:after, .on-off-switch:before
+{
+ font-family: Arial;
+ font-feature-settings: normal;
+ font-kerning: auto;
+ font-language-override: normal;
+ font-stretch: normal;
+ font-style: normal;
+ font-synthesis: weight style;
+ font-variant: normal;
+ font-weight: normal;
+ text-rendering: auto;
+}
+
+.on-off-switch label
+{
+ position: relative;
+ display: inline-block;
+
+ width: 90px;
+ height: 42px;
+ background: #ccc;
+ border-radius: 21px;
+ transition: 0.4s;
+}
+
+.on-off-switch label:after
+{
+ position: absolute;
+ left: 0;
+ top: -5px;
+ z-index: 2;
+
+ content: '';
+
+ width: 50px;
+ height: 50px;
+ background: #fff;
+ border-radius: 100%;
+ box-shadow: 0 0 5px rgba(0,0,0,.2);
+ transition: 0.4s;
+}
+
+.on-off-switch input
+{
+ position: absolute;
+ left: 0;
+ top: 0;
+ z-index: 5;
+
+ width: 100%;
+ height: 100%;
+ opacity: 0;
+ cursor: pointer;
+}
+
+.on-off-switch input:hover + label:after
+{
+ box-shadow: 0 2px 15px 0 rgba(0, 0, 0, .2), 0 3px 8px 0 rgba(0, 0, 0, .15);
+}
+
+.on-off-switch input:checked + label:after
+{
+ left: 40px;
+}
+
+
+
+
+
+/* =========================== */
+/* === TYPE 1 === */
+/* =========================== */
+.on-off-switch.type1 input:checked + label
+{
+ background: #376FCB;
+}
+
+.on-off-switch.type1 input:checked + label:after
+{
+ background: #4285F4;
+}
+
+
+
+
+
+/* =========================== */
+/* === TYPE 2 === */
+/* =========================== */
+.on-off-switch.type2:after
+{
+ position: absolute;
+ top: 0px;
+ right: 15px;
+
+ content: '';
+
+ color: #666;
+ font-family: Arial;
+ font-size: 25px;
+ line-height: 48px;
+ font-weight: bold;
+}
+
+.on-off-switch.type2 label
+{
+ width: 90px;
+ height: 40px;
+ background: none;
+ border:3px solid #777;
+ border-radius: 23px;
+}
+
+.on-off-switch.type2 label:after
+{
+ top: 4px;
+ left: auto;
+ right: 4px;
+ overflow: hidden;
+
+ content: 'YES';
+
+ width: 32px;
+ height: 32px;
+
+ color: #fff;
+ font-family: Arial;
+ font-size: 25px;
+ line-height: 32px;
+ text-align: center;
+ font-weight: bold;
+ text-indent: 100px;
+ background: #777;
+ box-shadow: none;
+ border-radius: 16px;
+
+ transform: translateX(-50px);
+ transition: all 0.4s 0.2s, width 0.2s linear, text-indent 0.4s linear;
+}
+
+.on-off-switch.type2 input:checked + label
+{
+ border-color: #329043;
+}
+
+.on-off-switch.type2 input:checked + label:after
+{
+ left: auto;
+
+ width: 82px;
+ text-indent: 0;
+ background: #3FB454;
+
+ transform: translateX(0px);
+ transition: all 0.4s, width 0.2s 0.4s linear, text-indent 0.3s 0.4s linear;
+}
+
+
+
+
+
+/* =========================== */
+/* === TYPE 2 === */
+/* =========================== */
+.on-off-switch.type2forstatusbar:after
+{
+ position: absolute;
+ top: 0px;
+ right: 12px;
+
+ content: '';
+
+ color: #ccc;
+ font-family: Arial;
+ font-size: 18px;
+ line-height: 40px;
+ font-weight: bold;
+}
+
+.on-off-switch.type2forstatusbar label
+{
+ width: 70px;
+ height: 32px;
+ background: none;
+ border:3px solid #ccc;
+ border-radius: 19px;
+}
+
+.on-off-switch.type2forstatusbar label:after
+{
+ top: 4px;
+ left: auto;
+ right: 4px;
+ overflow: hidden;
+
+ content: 'LIVE';
+
+ width: 24px;
+ height: 24px;
+
+ color: #fff;
+ font-family: Arial;
+ font-size: 18px;
+ line-height: 25px;
+ text-align: center;
+ font-weight: bold;
+ text-indent: 80px;
+ background: #ccc;
+ box-shadow: none;
+ border-radius: 12px;
+
+ transform: translateX(-38px);
+ transition: all 0.4s 0.2s, width 0.2s linear, text-indent 0.4s linear;
+}
+
+.on-off-switch.type2forstatusbar input:checked + label
+{
+ border-color: #269CE9;
+}
+
+.on-off-switch.type2forstatusbar input:checked + label:after
+{
+ left: auto;
+
+ width: 62px;
+ text-indent: 0;
+ background: #269CE9;
+
+ transform: translateX(0px);
+ transition: all 0.4s, width 0.2s 0.4s linear, text-indent 0.3s 0.4s linear;
+}
+
+
+
+
+
+/* =========================== */
+/* === TYPE 3 === */
+/* =========================== */
+.on-off-switch.type3 label
+{
+ width: 90px;
+ height: 42px;
+ border: 5px solid #555;
+ border-radius: 26px;
+ background: none;
+}
+
+.on-off-switch.type3 label:after
+{
+ top: 2px;
+ left: 2px;
+
+ width: 38px;
+ height: 38px;
+ background: #555;
+ box-shadow(none);
+}
+
+.on-off-switch.type3 input:checked + label
+{
+ border-color: #329043;
+}
+
+.on-off-switch.type3 input:checked + label:after
+{
+ left: 50px;
+ background: #3FB454;
+}
+
+
+
+
+
+/* =========================== */
+/* === TYPE 4 === */
+/* =========================== */
+.on-off-switch.type4:after
+{
+ position: absolute;
+ right: 15px;
+ bottom: 15px;
+
+ //content: '\f00c';
+ //content: '\274E';
+ //content: '\2715';
+ //content: '\2716';
+ //content: '\2717';
+ content: '\2718';
+ //content: '\274C';
+
+ color: #aaa;
+}
+
+.on-off-switch.type4:before
+{
+ position: absolute;
+ left: 15px;
+ bottom: 15px;
+
+ //content: '\f00d';
+ //content: '\2705';
+ //content: '\2713';
+ content: '\2714';
+
+ color: #fff;
+ z-index: 1;
+}
+
+.on-off-switch.type4 label
+{
+ width: 80px;
+ height: 42px;
+ border-radius: 21px;
+ background: #EAEAEA;
+ box-shadow: 0 0 1px 2px rgba(0,0,0,.15);
+}
+
+.on-off-switch.type4 label:after
+{
+ top: 0;
+
+ width: 42px;
+ height: 42px;
+}
+
+.on-off-switch.type4 input:checked + label
+{
+ background: #269CE9;
+}
+
+.on-off-switch.type4 input:checked + label:after
+{
+ left: 38px;
+}
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+
+ /* =========================== */
+ /* === PLAIN SWITCH === */
+ /* =========================== */
+ .on-off-switch-neighbouring-labelon-off-switch-neighbouring-label
+ {
+ line-height: 70px;
+ }
+
+ .on-off-switch label
+ {
+ width: 70px;
+ height: 32px;
+ border-radius: 16px;
+ }
+
+ .on-off-switch label:after
+ {
+ width: 40px;
+ height: 40px;
+ }
+
+ .on-off-switch input:checked + label:after
+ {
+ left: 30px;
+ }
+
+
+
+
+
+ /* =========================== */
+ /* === TYPE 2 === */
+ /* =========================== */
+ .on-off-switch.type2:after
+ {
+ top: 0px;
+ right: 12px;
+
+ content: '';
+
+ font-size: 18px;
+ line-height: 40px;
+ font-weight: bold;
+ }
+
+ .on-off-switch.type2 label
+ {
+ width: 70px;
+ height: 32px;
+ border:3px solid #777;
+ border-radius: 19px;
+ }
+
+ .on-off-switch.type2 label:after
+ {
+ top: 4px;
+ left: auto;
+ right: 4px;
+ overflow: hidden;
+
+ content: 'YES';
+
+ width: 24px;
+ height: 24px;
+
+ font-size: 18px;
+ line-height: 25px;
+ font-weight: bold;
+ text-indent: 80px;
+ border-radius: 12px;
+
+ transform: translateX(-38px);
+ }
+
+ .on-off-switch.type2 input:checked + label:after
+ {
+ width: 62px;
+ }
+
+
+
+
+
+ /* =========================== */
+ /* === TYPE 3 === */
+ /* =========================== */
+ .on-off-switch.type3 label
+ {
+ width: 70px;
+ height: 32px;
+ border: 5px solid #555;
+ border-radius: 21px;
+ }
+
+ .on-off-switch.type3 label:after
+ {
+ top: 2px;
+ left: 2px;
+
+ width: 28px;
+ height: 28px;
+ }
+
+ .on-off-switch.type3 input:checked + label:after
+ {
+ left: 40px;
+ }
+
+
+
+
+
+ /* =========================== */
+ /* === TYPE 4 === */
+ /* =========================== */
+ .on-off-switch.type4:after
+ {
+ right: 10px;
+ bottom: 10px;
+ }
+
+ .on-off-switch.type4:before
+ {
+ left: 10px;
+ bottom: 10px;
+ }
+
+ .on-off-switch.type4 label
+ {
+ width: 60px;
+ height: 32px;
+ border-radius: 16px;
+ }
+
+ .on-off-switch.type4 label:after
+ {
+ width: 32px;
+ height: 32px;
+ }
+
+ .on-off-switch.type4 input:checked + label:after
+ {
+ left: 28px;
+ }
+
+}
\ No newline at end of file
diff --git a/web_interface/html/css/switches_from_some_website.css b/web_interface/html/css/switches_from_some_website.css
new file mode 100644
index 0000000000000000000000000000000000000000..226c385a100017f1945e5f27cd960d5a7dbaff84
--- /dev/null
+++ b/web_interface/html/css/switches_from_some_website.css
@@ -0,0 +1,548 @@
+/*
+ TAKEN FROM:
+ https://codepen.io/vineethtrv/pen/QbqMXE
+*/
+
+@import "compass/css3";
+*, *:after, *:before{
+ @include box-sizing(border-box);
+}
+$blue : #4285F4;
+$green : #4BD865;
+$warning :#FF980F;
+
+body{
+ text-align: center;
+ background: #eee;
+}
+section{
+ float: left;
+ min-width: 150px;
+ width: 33.33%;
+ padding:25px 0;
+ min-height: 100px;
+}
+/*=====================*/
+.checkbox{
+ position: relative;
+ display: inline-block;
+
+ &:after, &:before{
+ font-family: FontAwesome;
+ font-feature-settings: normal;
+ font-kerning: auto;
+ font-language-override: normal;
+ font-stretch: normal;
+ font-style: normal;
+ font-synthesis: weight style;
+ font-variant: normal;
+ font-weight: normal;
+ text-rendering: auto;
+ }
+
+ label{
+ width: 90px;
+ height: 42px;
+ background: #ccc;
+ position: relative;
+ display: inline-block;
+ @include border-radius(46px);
+ @include transition(.4s);
+ &:after{
+ content: '';
+ position: absolute;
+ width: 50px;
+ height: 50px;
+ @include border-radius(100%);
+ left: 0;
+ top: -5px;
+ z-index: 2;
+ background: #fff;
+ @include box-shadow(0 0 5px rgba(0,0,0,.2));
+ @include transition(.4s);
+ }
+ }
+ input{
+ position: absolute;
+ left: 0;
+ top: 0;
+ width: 100%;
+ height: 100%;
+ z-index: 5;
+ opacity: 0;
+ cursor:pointer;
+ &:hover +label:after{
+ @include box-shadow(0 2px 15px 0 rgba(0, 0, 0, .2), 0 3px 8px 0 rgba(0, 0, 0, .15))
+ }
+ &:checked + label{
+ &:after{
+ left: 40px;
+ }
+ }
+
+ }
+}
+
+.model-1{
+ .checkbox{
+ input:checked + label{
+ background: $blue/1.2;
+ &:after{
+ background: $blue;
+ }
+ }
+ }
+}
+
+.model-2{
+ .checkbox{
+ label{
+ width: 75px;
+ &:after{
+ top: 0;
+ width:42px;
+ height: 42px;
+ }
+ }
+ input:checked + label{
+ background: $green;
+ &:after{
+ left: 35px;
+ }
+ }
+ }
+}
+
+.model-3{
+ .checkbox{
+ label{
+ background: #fff;
+ border:1px solid #eee;
+ height: 38px;
+ &:after{
+ background: #bbb;
+ top: 3px;
+ left: 5px;
+ width:30px;
+ height: 30px;
+ }
+ }
+ input:checked + label{
+ &:after{
+ background: $green/1.2;
+ left: 55px;
+ }
+ }
+ }
+}
+
+.model-4{
+ .checkbox{
+ label{
+ background: #bbb;
+ height: 25px;
+ width: 75px;
+ &:after{
+ background: #fff;
+ top: -8px;
+ width:38px;
+ height: 38px;
+ }
+ }
+ input:checked + label{
+ background: #77C2BB;
+ &:after{
+ background: #009688;
+ left: 40px;
+ }
+ }
+ }
+}
+.model-5{
+ .checkbox{
+ label{
+ background: #bbb;
+ height: 15px;
+ width: 85px;
+ &:after{
+ background: #fff;
+ top: -12px;
+ width:36px;
+ height: 36px;
+ }
+ }
+ input:hover + label:after{
+ @include scale(1.4);
+ }
+ input:checked + label{
+ background: $warning/1.4;
+ &:after{
+ background: $warning;
+ left: 50px;
+ }
+ }
+ }
+}
+.model-6{
+ .checkbox{
+ label{
+ background: #bbb;
+ height: 2px;
+ width: 60px;
+ &:after{
+ background: #bbb;
+ top: -16px;
+ width:32px;
+ height: 32px;
+ }
+ }
+ input:checked + label{
+ background: $blue/1.2;
+ &:after{
+ background:$blue;
+ left: 40px;
+ }
+ }
+ }
+}
+
+.model-7{
+ .checkbox{
+ label{
+ background: none;
+ border:5px solid #555;
+ height: 42px;
+ &:after{
+ background: #555;
+ @include box-shadow(none);
+ top: 2px;
+ left: 2px;
+ width:28px;
+ height: 28px;
+ }
+ }
+ input:checked + label{
+ border-color:$green/1.5;
+ &:after{
+ background: $green/1.2;
+ left: 50px;
+ }
+ }
+ }
+}
+.model-8{
+ .checkbox{
+ label{
+ background: #ddd;
+ width: 95px;
+ @include border-radius(10px);
+ &:after{
+ background: #fff;
+ @include border-radius(10px);
+ top: 0;
+ width:60px;
+ height: 42px;
+ }
+ }
+ input:checked + label{
+ background:$warning;
+ &:after{
+ left: 35px;
+ }
+ }
+ }
+}
+.model-9{
+ .checkbox{
+ label{
+ background: #aaa;
+ width: 90px;
+ height: 32px;
+ @include border-radius(20px);
+ &:after{
+ @include border-radius(20px);
+ top: 0;
+ width:50px;
+ height: 32px;
+ }
+ }
+ input:checked + label{
+ background:$green/2;
+ &:after{
+ background: $green/1.4;
+ left: 40px;
+ }
+ }
+ }
+}
+.model-10{
+ .checkbox{
+ &:after{
+ content: '\f00d';
+ color: #aaa;
+ position: relative;
+ right: 30px;
+ bottom: 15px;
+ }
+ &:before{
+ content: '\f00c';
+ position: relative;
+ left: 35px;
+ bottom: 15px;
+ color: #fff;
+ z-index: 1;
+ }
+ label{
+ width: 80px;
+ background: #EAEAEA;
+ @include box-shadow(0 0 1px 2px rgba(0,0,0,.15));
+ &:after{
+ top: 0;
+ width:42px;
+ height: 42px;
+ }
+ }
+ input:checked + label{
+ background: $green;
+ &:after{
+ left: 40px;
+ }
+ }
+ }
+}
+
+.model-11{
+ .checkbox{
+ &:after, &:before{
+ content: 'OFF';
+ position: absolute;
+ right: 10px;
+ top: 10px;
+ font-family: Arial, "Helvetica Neue", Helvetica, sans-serif;
+ font-size: 12px;
+ color: #90201F;
+ }
+ &:before{
+ content: 'ON';
+ left: -40px;
+ z-index: 1;
+ color: $green/2;
+ }
+ label{
+ background: #E3666C;
+ height: 32px;
+ @include border-radius(0);
+ @include box-shadow(0 0 1px 2px rgba(0,0,0,.2));
+ &:after{
+ background-color: #ffffff;
+ @include filter-gradient(#ffffff, #ffffff, horizontal);
+ @include background-image(linear-gradient(left, #fff 30%,#ddd 45%,#fff 50%,#ddd 55%,#fff 70%));
+ @include border-radius(0);
+ @include box-shadow(none !important);
+ @include transition(.1s);
+ top: 0;
+ width:50px;
+ height: 32px;
+ }
+ }
+ input:checked + label{
+ background:$green;
+ &:after{
+ left: 40px;
+ }
+ }
+ }
+}
+
+
+.model-12{
+ .checkbox{
+ background: #2B2B2D;
+ height: 40px;
+ width: 160px;
+ @include border-radius(50px);
+ &:after, &:before{
+ content: 'ON';
+ font-family: Arial, "Helvetica Neue", Helvetica, sans-serif;
+ line-height: 40px;
+ font-size: 12px;
+ }
+ &:before{
+ content: 'OFF';
+ z-index: 1;
+ }
+ label{
+ background: #1E1E1E;
+ height:10px;
+ width: 70px;
+ margin: 0 5px;
+ @include box-shadow(0 0 2px 2px rgba(0,0,0,.3) inset) ;
+ &:after{
+ background-color: #3F4545;
+ @include background-image(linear-gradient(top, #666 0%,#3F4545 100%));
+ top: -9px;
+ width:27px;
+ height: 27px;
+ }
+ &:before{
+ content: '';
+ position: absolute;
+ width: 14px;
+ height: 14px;
+ @include border-radius(100%);
+ @include transition(.4s);
+ background:#151515;
+ z-index: 3;
+ left: 7px;
+ top: -2px;
+ }
+ }
+ input:checked + label{
+ &:after{
+ left: 45px;
+ }
+ &:before{
+ background: #94DA00;
+ @include box-shadow(0 0 5px #94DA00) ;
+ left: 52px;
+ }
+ }
+ }
+}
+.model-13{
+ .checkbox{
+ &:after{
+ content: 'NO';
+ font-family: Arial;
+ position: absolute;
+ color: #666;
+ top: 12px;
+ right: 15px;
+ }
+ label{
+ background: none;
+ border:3px solid #777;
+ height: 40px;
+ @include border-radius(20px);
+
+ &:after{
+ content: 'YES';
+ font-family: Arial;
+ color: #fff;
+ line-height: 28px;
+ text-indent: 100px;
+ background: #777;
+ overflow: hidden;
+ @include box-shadow(none);
+ @include border-radius(14px);
+ @include translateX(-50px);
+ @include transition(all .4s .2s, width .2s linear, text-indent .4s linear);
+
+ top: 3px;
+ left: auto;
+ right: 2px;
+ width:28px;
+ height: 28px;
+
+ }
+ }
+ input:checked + label{
+ border-color:$green/1.5;
+ &:after{
+ background: $green/1.2;
+ left: auto;
+ @include translateX(0px);
+ @include transition(all .4s, width .2s .4s linear, text-indent .3s .4s linear);
+ width: 80px;
+ text-indent: 0;
+ }
+ }
+ }
+}
+
+.model-14{
+ .checkbox{
+ &:after, &:before{
+ content: 'OFF';
+ position: absolute;
+ right: 10px;
+ top: 10px;
+ font-family: Arial, "Helvetica Neue", Helvetica, sans-serif;
+ font-size: 12px;
+ color: #90201F;
+ }
+ &:before{
+ content: 'ON';
+ left: -40px;
+ z-index: 1;
+ color: $green/2;
+ }
+ label{
+ background:#fff;
+ height: 32px;
+ @include border-radius(0);
+ @include box-shadow(0 0 2px rgba(0,0,0,.2));
+ &:after{
+ background: #90201F;
+ @include border-radius(0);
+ @include box-shadow(none !important);
+ @include transition(.3s);
+ top: 0;
+ width:40px;
+ height: 32px;
+ }
+ }
+ input:checked + label{
+ &:after{
+ background:$green;
+ left: 50px;
+ }
+ }
+ }
+}
+.model-15{
+ .checkbox{
+ width: 94px;
+ height: 34px;
+ border: 2px solid #ddd;
+ background:$green/2;
+ @include border-radius(6px);
+ overflow: hidden;
+ &:after, label:before{
+ content: 'ON';
+ position: absolute;
+ left: 10px;
+ top: 8px;
+ font-family: Arial, "Helvetica Neue", Helvetica, sans-serif;
+ font-size: 12px;
+ color: #fff;
+ }
+ label:before{
+ content: 'OFF';
+ left:auto;
+ top: 8px;
+ right: 10px;
+ z-index: 1;
+ }
+ label{
+ background: #f00;
+ width: 90px;
+ height: 32px;
+ @include border-radius(4px);
+ &:after{
+ @include border-radius(4px);
+ @include box-shadow(none !important);
+ top: 0;
+ width:50px;
+ height: 32px;
+ }
+ }
+ input:checked + label{
+ @include translateX(40px);
+ &:after{
+ left:0;
+ }
+ }
+ }
+}
+
diff --git a/web_interface/html/css/tabs_control.css b/web_interface/html/css/tabs_control.css
new file mode 100644
index 0000000000000000000000000000000000000000..7d0c8c4eaafdef028c7c12526fb3211698513973
--- /dev/null
+++ b/web_interface/html/css/tabs_control.css
@@ -0,0 +1,128 @@
+.control-tabs
+{
+ display: flex;
+ flex-wrap: wrap;
+ background: #efefef;
+ border: 1pt solid #34495E;
+ box-shadow: 0 48px 80px -32px rgba(0,0,0,0.3);
+ //top: 0px;
+ //bottom: 0px;
+ margin: 0px;
+}
+
+.control-tab-input {
+ position: absolute;
+ opacity: 0;
+}
+
+.control-tab-label
+{
+ width: auto;
+ padding-left: 30px;
+ padding-right: 30px;
+ padding-top: 20px;
+ padding-bottom: 20px;
+ background: #e5e5e5;
+ cursor: pointer;
+ font-weight: bold;
+ font-size: 18px;
+ color: #7f7f7f;
+ transition: background 0.0s, color 0.0s;
+}
+
+.control-tab-label:hover
+{
+ background: #d8d8d8;
+}
+
+.control-tab-label:active
+{
+ background: #ccc;
+}
+
+.control-tab-input:focus + .control-tab-label
+{
+ /*box-shadow: inset 0px 0px 0px 3px #2aa1c0*/;
+ z-index: 1;
+}
+
+.control-tab-input:checked + .control-tab-label
+{
+ background: #fff;
+ color: #000;
+}
+
+.control-tab-panel
+{
+ order: 99;
+ display: none;
+ position: relative;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ margin-right: 0px;
+ width:100%;
+ padding-left: 20px;
+ padding-right: 20px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ background: #fff;
+ color:black;
+ overflow: hidden;
+ overflow-y: scroll;
+ border-left: 1px solid;
+ border-right: 1px solid;
+ border-width: 10px;
+ border-color: #c43c35;
+ /*
+ border-color: #c43c35;
+ border-color: #f44336;
+ border-color: #57a957;
+ */
+}
+
+/* Make the respective panel visible when its tab-input is checked */
+.control-tab-input:checked + .control-tab-label + .control-tab-panel
+{
+ display: block;
+}
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 600px)
+{
+ /* Make it an accordian */
+ .control-tab-label
+ {
+ width: 50%;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 20px;
+ padding-bottom: 20px;
+ text-align: center;
+ }
+}
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ /* Make it an accordian */
+ .control-tab-label
+ {
+ width: 50%;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 15px;
+ padding-bottom: 15px;
+ text-align: center;
+ }
+
+ .control-tab-panel
+ {
+ padding-left: 10px;
+ padding-right: 10px;
+ }
+
+}
\ No newline at end of file
diff --git a/web_interface/html/css/tabs_home.css b/web_interface/html/css/tabs_home.css
new file mode 100644
index 0000000000000000000000000000000000000000..0521ec250201be97c8052ba2ec61d3d73947c1b4
--- /dev/null
+++ b/web_interface/html/css/tabs_home.css
@@ -0,0 +1,135 @@
+.home-tabs
+{
+ display: flex;
+ flex-wrap: wrap;
+ background: #efefef;
+ //box-shadow: 0 48px 80px -32px rgba(0,0,0,0.3);
+ top: 0px;
+ bottom: 0px;
+ margin: 0px;
+}
+
+.home-tab-input {
+ position: absolute;
+ opacity: 0;
+}
+
+.home-tab-label
+{
+ width: auto;
+ padding-left: 30px;
+ padding-right: 30px;
+ padding-top: 20px;
+ padding-bottom: 20px;
+ background: #e5e5e5;
+ cursor: pointer;
+ font-weight: bold;
+ font-size: 18px;
+ color: #7f7f7f;
+ transition: background 0.0s, color 0.0s;
+}
+
+.home-tab-label:hover
+{
+ background: #d8d8d8;
+}
+
+.home-tab-label:active
+{
+ background: #ccc;
+}
+
+.home-tab-input:focus + .home-tab-label
+{
+ /*box-shadow: inset 0px 0px 0px 3px #2aa1c0*/;
+ z-index: 1;
+}
+
+.home-tab-input:checked + .home-tab-label
+{
+ background: #fff;
+ color: #000;
+}
+
+.home-tab-panel
+{
+ order: 99;
+ display: none;
+ position: absolute;
+ top: 0;
+ bottom: 0;
+ left: 0px;
+ right: 0px;
+ top: 235px;
+ bottom: 0px;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ margin-right: 0px;
+ max-width: 800px;
+ padding-left: 20px;
+ padding-right: 20px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ background: #fff;
+ color:black;
+ overflow: hidden;
+ overflow-y: auto;
+}
+
+.home-tab-panel.thin-padding
+{
+ padding-left: 10px;
+ padding-right: 10px;
+}
+
+/* Make the respective panel visible when its tab-input is checked */
+.home-tab-input:checked + .home-tab-label + .home-tab-panel
+{
+ display: block;
+}
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 600px)
+{
+ /* Make it an accordian */
+ .home-tab-label
+ {
+ width: 25%;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 20px;
+ padding-bottom: 20px;
+ text-align: center;
+ }
+}
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ /* Make it an accordian */
+ .home-tab-label
+ {
+ width: 50%;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ text-align: center;
+ }
+
+ .home-tab-panel
+ {
+ top: 185px;
+ }
+
+ .home-tab-panel.thin-padding
+ {
+ padding-left: 5px;
+ padding-right: 5px;
+ }
+
+}
\ No newline at end of file
diff --git a/web_interface/html/css/tabs_main.css b/web_interface/html/css/tabs_main.css
new file mode 100644
index 0000000000000000000000000000000000000000..0fb1582f0727ec220dfd22952aa750e4324112ca
--- /dev/null
+++ b/web_interface/html/css/tabs_main.css
@@ -0,0 +1,155 @@
+.main-tabs
+{
+ display: flex;
+ flex-wrap: wrap;
+ background: #efefef;
+ //box-shadow: 0 48px 80px -32px rgba(0,0,0,0.3);
+ top: 0px;
+ bottom: 0px;
+ margin: 0px;
+}
+
+.main-tab-input {
+ position: absolute;
+ opacity: 0;
+}
+
+.main-tab-label
+{
+ width: auto;
+ padding-left: 30px;
+ padding-right: 30px;
+ padding-top: 20px;
+ padding-bottom: 20px;
+ background: #e5e5e5;
+ cursor: pointer;
+ font-weight: bold;
+ font-size: 18px;
+ color: #7f7f7f;
+ transition: background 0.0s, color 0.0s;
+}
+
+.main-tab-label:hover
+{
+ background: #d8d8d8;
+}
+
+.main-tab-label:active
+{
+ background: #ccc;
+}
+
+.main-tab-input:focus + .main-tab-label
+{
+ /*box-shadow: inset 0px 0px 0px 3px #2aa1c0*/;
+ z-index: 1;
+}
+
+.main-tab-input:checked + .main-tab-label
+{
+ background: #fff;
+ color: #000;
+}
+
+/* The "top" specification here is critical */
+/* Use: */
+/* 189px when the top banner contains the */
+/* motors-off button and agent IP */
+/* 190px when the top banner contains the */
+/* motors-off button and status */
+/* icons */
+/* 229px when the top banner contains the */
+/* motors-off button, agent IP, and */
+/* status icons. */
+.main-tab-panel
+{
+ order: 99;
+ display: none;
+ position: absolute;
+ top: 0;
+ bottom: 0;
+ left: 0px;
+ right: 0px;
+ top: 190px;
+ bottom: 0px;
+ margin-top: 0px;
+ margin-bottom: 0px;
+ margin-left: 0px;
+ margin-right: 0px;
+ max-width: 800px;
+ padding-left: 20px;
+ padding-right: 20px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ background: #fff;
+ color:black;
+ overflow: hidden;
+ overflow-y: auto;
+}
+
+.main-tab-panel.thin-padding
+{
+ padding-left: 10px;
+ padding-right: 10px;
+}
+
+/* Make the respective panel visible when its tab-input is checked */
+.main-tab-input:checked + .main-tab-label + .main-tab-panel
+{
+ display: block;
+}
+
+
+
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 600px)
+{
+ /* Make it an accordian */
+ .main-tab-label
+ {
+ width: 25%;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 20px;
+ padding-bottom: 20px;
+ text-align: center;
+ }
+}
+
+/* SETTINGS FOR WHEN THE SCREEN IS SMALL */
+@media screen and (max-width: 420px)
+{
+ /* Make it an accordian */
+ .main-tab-label
+ {
+ width: 25%;
+ padding-left: 0px;
+ padding-right: 0px;
+ padding-top: 10px;
+ padding-bottom: 10px;
+ text-align: center;
+ }
+
+ /* The "top" specification here is critical */
+ /* Use: */
+ /* 147px when the top banner contains the */
+ /* motors-off button and agent IP */
+ /* 162px when the top banner contains the */
+ /* motors-off button and status */
+ /* icons */
+ /* 190px when the top banner contains the */
+ /* motors-off button, agent IP, and */
+ /* status icons. */
+ .main-tab-panel
+ {
+ top: 162px;
+ }
+
+ .main-tab-panel.thin-padding
+ {
+ padding-left: 5px;
+ padding-right: 5px;
+ }
+
+}
\ No newline at end of file
diff --git a/web_interface/html/home_tab_about.html b/web_interface/html/home_tab_about.html
new file mode 100644
index 0000000000000000000000000000000000000000..fdceac46c3b02e0aa99a8ab4c149d0ca8f4c1693
--- /dev/null
+++ b/web_interface/html/home_tab_about.html
@@ -0,0 +1,11 @@
+<body>
+ <div class="body-container">
+ <main style="text-align:left;">
+
+ <br>
+
+ Coming soon...
+
+ </main>
+ </div>
+</body>
\ No newline at end of file
diff --git a/web_interface/html/home_tab_enter.html b/web_interface/html/home_tab_enter.html
new file mode 100644
index 0000000000000000000000000000000000000000..9cbf127f892620c7e8849930f5fdd07de1e54251
--- /dev/null
+++ b/web_interface/html/home_tab_enter.html
@@ -0,0 +1,33 @@
+<body>
+ <div class="body-container">
+ <main style="text-align:left;">
+
+ <br>
+
+ <button class="button-push navy" onclick="window.location.href='agent.php'">
+ Enter as Agent
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+
+ <br>
+
+ <p>
+ The agent interface allows you to transfer and compile code, launch the ROS nodes, and control the agent.
+ </p>
+
+ <br><br><br>
+
+ <button class="button-push navy" onclick="window.location.href='coord.php'">
+ Enter as Coordinator
+ <div class="div-for-button-highlight-on-touchscreen navy"></div>
+ </button>
+
+ <br>
+
+ <p>
+ The coordinator interface allows you to compile code, launch the ROS nodes, and control multiple agents simultaneously.
+ </p>
+
+ </main>
+ </div>
+</body>
\ No newline at end of file
diff --git a/web_interface/html/img/battery_20.png b/web_interface/html/img/battery_20.png
new file mode 100644
index 0000000000000000000000000000000000000000..cc7ae62ab7c662f7cf3e098e713232d2d6e0ae14
Binary files /dev/null and b/web_interface/html/img/battery_20.png differ
diff --git a/web_interface/html/img/battery_40.png b/web_interface/html/img/battery_40.png
new file mode 100644
index 0000000000000000000000000000000000000000..cea5ab35b9605910112dd8a7e2eda371c430894b
Binary files /dev/null and b/web_interface/html/img/battery_40.png differ
diff --git a/web_interface/html/img/battery_60.png b/web_interface/html/img/battery_60.png
new file mode 100644
index 0000000000000000000000000000000000000000..1f75257c3c1f44fc10f318274741942916b47b70
Binary files /dev/null and b/web_interface/html/img/battery_60.png differ
diff --git a/web_interface/html/img/battery_80.png b/web_interface/html/img/battery_80.png
new file mode 100644
index 0000000000000000000000000000000000000000..8224e7e1e2ed8e251f37f802b14ce4ebd9027f35
Binary files /dev/null and b/web_interface/html/img/battery_80.png differ
diff --git a/web_interface/html/img/battery_empty.png b/web_interface/html/img/battery_empty.png
new file mode 100644
index 0000000000000000000000000000000000000000..0bf35549aab7c7c0247cebd69a176d68ea67387c
Binary files /dev/null and b/web_interface/html/img/battery_empty.png differ
diff --git a/web_interface/html/img/battery_full.png b/web_interface/html/img/battery_full.png
new file mode 100644
index 0000000000000000000000000000000000000000..495383be5e346e624d7a92670fcf9ddb053f99ef
Binary files /dev/null and b/web_interface/html/img/battery_full.png differ
diff --git a/web_interface/html/img/battery_unavailable.png b/web_interface/html/img/battery_unavailable.png
new file mode 100644
index 0000000000000000000000000000000000000000..71765cfe18b1d7a915b7f92daf06df3b6558521f
Binary files /dev/null and b/web_interface/html/img/battery_unavailable.png differ
diff --git a/web_interface/html/img/battery_unknown.png b/web_interface/html/img/battery_unknown.png
new file mode 100644
index 0000000000000000000000000000000000000000..e20348f65a39bac7c0e40a2a4c0b338ef018dc03
Binary files /dev/null and b/web_interface/html/img/battery_unknown.png differ
diff --git a/web_interface/html/img/flying_state_disabling.png b/web_interface/html/img/flying_state_disabling.png
new file mode 100644
index 0000000000000000000000000000000000000000..667c8027f2b902f08f8e81d3120599a5ed151d48
Binary files /dev/null and b/web_interface/html/img/flying_state_disabling.png differ
diff --git a/web_interface/html/img/flying_state_enabling.png b/web_interface/html/img/flying_state_enabling.png
new file mode 100644
index 0000000000000000000000000000000000000000..ef2dc5bffa6d27edbb6c6ff1171cd77a5af5c20a
Binary files /dev/null and b/web_interface/html/img/flying_state_enabling.png differ
diff --git a/web_interface/html/img/flying_state_flying.png b/web_interface/html/img/flying_state_flying.png
new file mode 100644
index 0000000000000000000000000000000000000000..b0b51f3d42edbfad9f875c1faa413391c4f21de9
Binary files /dev/null and b/web_interface/html/img/flying_state_flying.png differ
diff --git a/web_interface/html/img/flying_state_off.png b/web_interface/html/img/flying_state_off.png
new file mode 100644
index 0000000000000000000000000000000000000000..70669664f07c12ef27a41f5b5fb918aff93da688
Binary files /dev/null and b/web_interface/html/img/flying_state_off.png differ
diff --git a/web_interface/html/img/flying_state_unavailable.png b/web_interface/html/img/flying_state_unavailable.png
new file mode 100644
index 0000000000000000000000000000000000000000..60f6878b603abc0ba5dd375765ff06ad36a397ab
Binary files /dev/null and b/web_interface/html/img/flying_state_unavailable.png differ
diff --git a/web_interface/html/img/flying_state_unknown.png b/web_interface/html/img/flying_state_unknown.png
new file mode 100644
index 0000000000000000000000000000000000000000..5c0c7337a103e5726af53def62e37ad22209c972
Binary files /dev/null and b/web_interface/html/img/flying_state_unknown.png differ
diff --git a/web_interface/html/img/rf_connected.png b/web_interface/html/img/rf_connected.png
new file mode 100644
index 0000000000000000000000000000000000000000..f9f3580113053f71fe86344e6f34a74e98c17b70
Binary files /dev/null and b/web_interface/html/img/rf_connected.png differ
diff --git a/web_interface/html/img/rf_connecting.png b/web_interface/html/img/rf_connecting.png
new file mode 100644
index 0000000000000000000000000000000000000000..5f1d4ca1b48496ecf1372e1a0f727058bbc24d95
Binary files /dev/null and b/web_interface/html/img/rf_connecting.png differ
diff --git a/web_interface/html/img/rf_disconnected.png b/web_interface/html/img/rf_disconnected.png
new file mode 100644
index 0000000000000000000000000000000000000000..0e99e624009059328820b317c908d337caf55ddc
Binary files /dev/null and b/web_interface/html/img/rf_disconnected.png differ
diff --git a/web_interface/html/index.php b/web_interface/html/index.php
new file mode 100644
index 0000000000000000000000000000000000000000..80216f485e17899c2ca4c2fa058d1ffdc569b76b
--- /dev/null
+++ b/web_interface/html/index.php
@@ -0,0 +1,60 @@
+<?php
+ include("page_header.html");
+?>
+
+<div class="full-window-fixed">
+</div>
+
+<div class="max-width-full-heigth-fixed">
+
+ <div class="top-bar-container">
+ <table class="top-bar-buttons-table">
+ <tr>
+ <td class="top-bar-motorsoff-button-cell">
+ <button class="button-push red motorsoff" disabled onclick="checkForRosAgent()">
+ MOTORS-OFF
+ </button>
+ </td>
+ <td class="top-bar-info-button-cell">
+ <button class="button-push blue info" onclick="checkForRosAgent()">
+ i
+ <div class="div-for-button-highlight-on-touchscreen blue"></div>
+ </button>
+ </td>
+ </tr>
+ </table>
+
+ <div class="top-bar-title">
+ Browser Interface
+ </div>
+ <div class="top-bar-subtitle">
+ for the D-FaLL System
+ </div>
+ <div class="top-bar-subtitle padbelow">
+ Connected to IP <?php echo $_SERVER['REMOTE_ADDR']; ?>
+ </div>
+ </div>
+
+ <div class="home-tabs">
+ <input name="tabs" type="radio" id="home-tab-1" checked="checked" class="home-tab-input"/>
+ <label for="home-tab-1" class="home-tab-label">Enter</label>
+ <div class="home-tab-panel">
+ <?php
+ include("home_tab_enter.html");
+ ?>
+ </div>
+
+ <input name="tabs" type="radio" id="home-tab-2" class="home-tab-input"/>
+ <label for="home-tab-2" class="home-tab-label">About</label>
+ <div class="home-tab-panel">
+ <?php
+ include("home_tab_about.html");
+ ?>
+ </div>
+ </div>
+</div>
+
+
+<?php
+ include("page_footer.html");
+?>
\ No newline at end of file
diff --git a/web_interface/html/js/callbashscript.js b/web_interface/html/js/callbashscript.js
new file mode 100644
index 0000000000000000000000000000000000000000..e494f3c1f444c1ef6b1b524b8c1dc65427cdd9f5
--- /dev/null
+++ b/web_interface/html/js/callbashscript.js
@@ -0,0 +1,169 @@
+function callBashScript_outputLabelID_clearOtherLabels_clickOtherButtons(bashscript, labelID, otherLabels, otherButtons)
+{
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+ // Add the function to be run when the response is recieved
+ xmlhttp.onreadystatechange = function()
+ {
+ if (this.readyState == 4 && this.status == 200)
+ {
+ // Construct the return string
+ var base_string = "";
+ document.getElementById(labelID).innerHTML = base_string + this.responseText;
+ // Click the "other buttons" as requested
+ if (otherButtons)
+ {
+ if ( typeof(otherButtons) == "string" )
+ {
+ document.getElementById(otherButtons).click();
+ }
+ else if (otherButtons.constructor === Array)
+ {
+ for (otherButtonID of otherButtons)
+ {
+ if (otherButtonID)
+ {
+ if ( typeof(otherButtonID) == "string" )
+ {
+ document.getElementById(otherButtonID).click();
+ }
+ }
+ }
+ }
+ }
+
+ }
+ else
+ {
+ var display_message = getDisplayMessageForXMLHttpRequest(this);
+ document.getElementById(labelID).innerHTML = display_message;
+ }
+ };
+ xmlhttp.open("GET", "callBashScript.php?scriptname=" + bashscript, true);
+ xmlhttp.send();
+
+ // Clear the other labels as requested
+ if (otherLabels)
+ {
+ if ( typeof(otherLabels) == "string" )
+ {
+ document.getElementById(otherLabels).innerHTML = "";
+ }
+ else if (otherLabels.constructor === Array)
+ {
+ for (otherLabelID of otherLabels)
+ {
+ if (otherLabelID)
+ {
+ if ( typeof(otherLabelID) == "string" )
+ {
+ document.getElementById(otherLabelID).innerHTML = "";
+ }
+ }
+ }
+ }
+ }
+}
+
+
+
+function getDisplayMessageForXMLHttpRequest(xmlhttp)
+{
+ var base_string = "";
+ if (xmlhttp.readyState != 4)
+ {
+ base_string = base_string + "loading...";
+ }
+ else
+ {
+ if (xmlhttp.status != 204)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": no content.";
+ }
+ else if (xmlhttp.status != 403)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": request forbidden.";
+ }
+ else if (xmlhttp.status != 404)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": page not found.";
+ }
+ else if (xmlhttp.status != 500)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": internal server error.";
+ }
+ else if (xmlhttp.status != 501)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": no implemented.";
+ }
+ else if (xmlhttp.status != 502)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": bd gateway.";
+ }
+ else if (xmlhttp.status != 503)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": service unavailable.";
+ }
+ else if (xmlhttp.status != 504)
+ {
+ base_string = base_string + "Error " + xmlhttp.status + ": gateway timeout.";
+ }
+ else
+ {
+ base_string = base_string + "Error " + xmlhttp.status;
+ }
+
+ }
+ // Finally return the string
+ return base_string;
+}
+
+
+function getErrorMessageForXMLHttpRequest(xmlhttp)
+{
+ var base_string = "";
+ if (xmlhttp.readyState != 4 && xmlhttp.status != 200)
+ {
+ base_string = base_string + "readyState = " + xmlhttp.readyState + " and status = " + xmlhttp.status;
+ }
+ else if (xmlhttp.readyState != 4)
+ {
+ base_string = base_string + "status is good but readyState = " + xmlhttp.readyState;
+ }
+ else if (xmlhttp.status != 200)
+ {
+ base_string = base_string + "ready state is good but status = " + xmlhttp.status;
+ }
+ // Finally return the string
+ return base_string;
+}
+
+/*
+The "XMLHttpRequest" variable has the following:
+> CALLBACK FUNCTION: onreadystatechange
+ Defines a function to be called when the readyState property changes
+
+> PROPERTY: readyState
+ Holds the status of the XMLHttpRequest.
+ 0: request not initialized
+ 1: server connection established
+ 2: request received
+ 3: processing request
+ 4: request finished and response is ready
+
+> PROPERTY status
+ 200: "OK"
+ 403: "Forbidden"
+ 404: "Page not found"
+ For a complete list go to the Http Messages Reference
+
+> PROPERTY statusText
+ Returns the status-text (e.g. "OK" or "Not Found")
+
+> PROPERTY responseText
+ Returns the server response as a JavaScript string
+
+For more details see:
+https://www.w3schools.com/xml/ajax_xmlhttprequest_response.asp
+https://www.w3schools.com/tags/ref_httpmessages.asp
+*/
diff --git a/web_interface/html/js/callgit.js b/web_interface/html/js/callgit.js
new file mode 100644
index 0000000000000000000000000000000000000000..a23174c6fb7de4f60457a8f633b6488043acd1bb
--- /dev/null
+++ b/web_interface/html/js/callgit.js
@@ -0,0 +1,127 @@
+function callGit_outputLabelID_shouldAppend_shouldConfirm(gitCommand, labelID, shouldAppend, shouldConfirm)
+{
+ // Convert the "gitCommand" to a heding string
+ var heading_string = "<p class=\"terminal-highligh-paragraph\">";
+ var scriptname_for_php = "";
+ var confirm_alert_text = "";
+ if (gitCommand == "status")
+ {
+ heading_string = heading_string + "GIT STATUS";
+ scriptname_for_php = scriptname_for_php + "gitStatus";
+ confirm_alert_text = confirm_alert_text + "This will check the status of the repository.";
+ }
+ else if (gitCommand == "pull")
+ {
+ heading_string = heading_string + "GIT PULL";
+ scriptname_for_php = scriptname_for_php + "gitPull";
+ confirm_alert_text = confirm_alert_text + "This will pull the latest changes, and may fail if there are any conflicts.";
+ }
+ else if (gitCommand == "diff")
+ {
+ heading_string = heading_string + "GIT DIFF";
+ scriptname_for_php = scriptname_for_php + "gitDiff";
+ confirm_alert_text = confirm_alert_text + "This will show the difference between the current status and the previous commit.";
+ }
+ else if (gitCommand == "checkout")
+ {
+ heading_string = heading_string + "GIT CHECKOUT .";
+ scriptname_for_php = scriptname_for_php + "gitCheckout";
+ confirm_alert_text = confirm_alert_text + "This remove any changes and those changes can NOT be recovered.";
+ }
+ else if (gitCommand == "checkoutall")
+ {
+ heading_string = heading_string + "GIT CHECKOUT .";
+ scriptname_for_php = scriptname_for_php + "gitCheckoutAll";
+ confirm_alert_text = confirm_alert_text + "This remove any changes and those changes can NOT be recovered.";
+ }
+ else if (gitCommand == "checkoutmaster")
+ {
+ heading_string = heading_string + "GIT CHECKOUT MASTER";
+ scriptname_for_php = scriptname_for_php + "gitCheckoutMaster";
+ confirm_alert_text = confirm_alert_text + "This switches branch and the web interface does not offer the functionality to switch back.";
+ }
+ else if (gitCommand == "catkin_make")
+ {
+ heading_string = heading_string + "CATKIN_MAKE";
+ scriptname_for_php = scriptname_for_php + "catkin_make";
+ confirm_alert_text = confirm_alert_text + "This compiles the dfall-system code.";
+ }
+ // Add the end paragraph tag to the heading string
+ heading_string = heading_string + "</p>";
+
+ // Check the "shouldAppend" input argument
+ if ( typeof(shouldAppend) !== "boolean" )
+ {
+ shouldAppend = true;
+ }
+
+ // Check the "shouldConfirm" input argument
+ if ( typeof(shouldConfirm) !== "boolean" )
+ {
+ shouldConfirm = true;
+ }
+
+ // If required, confirm the user wants to perform this action
+ if (shouldConfirm)
+ {
+ var alert_response_bool = confirm(confirm_alert_text);
+ if (alert_response_bool == false)
+ {
+ // If false then the user pressed Cancel
+ return;
+ }
+ }
+
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+ // Add the function to be run when the response is recieved
+ xmlhttp.onreadystatechange = function()
+ {
+ if (this.readyState == 4 && this.status == 200)
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ if (shouldAppend)
+ {
+ // Take a copy of the current contents
+ var current_contents = document.getElementById(labelID).innerHTML;
+ // Append the new contents to the start
+ document.getElementById(labelID).innerHTML = base_string + heading_string + "<br>" + this.responseText + "<br><br>" + current_contents;
+ }
+ else
+ {
+ document.getElementById(labelID).innerHTML = base_string + heading_string + "<br>" + this.responseText;
+ }
+
+ }
+ else
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ var display_message = getDisplayMessageForXMLHttpRequest(this);
+ if (shouldAppend)
+ {
+ if (this.readyState == 4)
+ {
+ // Take a copy of the current contents
+ var current_contents = document.getElementById(labelID).innerHTML;
+ // Append the new contents to the start
+ document.getElementById(labelID).innerHTML = base_string + heading_string + "<br>" + display_message + "<br><br>" + current_contents;
+ }
+ }
+ else
+ {
+ if (this.readyState == 4)
+ {
+ document.getElementById(labelID).innerHTML = base_string + heading_string + "<br>" + display_message;
+ }
+ else
+ {
+ document.getElementById(labelID).innerHTML = base_string + display_message;
+ }
+ }
+ }
+ };
+ xmlhttp.open("GET", "callBashScript.php?scriptname=" + scriptname_for_php, true);
+ xmlhttp.send();
+}
\ No newline at end of file
diff --git a/web_interface/html/js/fileinput.js b/web_interface/html/js/fileinput.js
new file mode 100644
index 0000000000000000000000000000000000000000..82f5db061996becd9e65348ef0c34b69879e1b5c
--- /dev/null
+++ b/web_interface/html/js/fileinput.js
@@ -0,0 +1,150 @@
+function initializeFileInputs()
+{
+ // Get the input fields into variables
+ var cppfileinput = document.getElementById("cppfileinput");
+ var yamlfileinput = document.getElementById("yamlfileinput");
+
+ // Add an event listener to each
+ cppfileinput.addEventListener( "change", cppFileInputChangedEvent, false);
+ yamlfileinput.addEventListener("change", yamlFileInputChangedEvent, false);
+}
+
+
+
+function cppFileInputChangedEvent(event)
+{
+ // Get the file list object
+ // > There should only be one file
+ var files = event.target.files;
+
+ // Parse the file
+ parseFileDetails( files[0] , "cppfiledetails" );
+
+ // Upload the file
+ uploadFile( files[0] , "text/x-c++src" , "cppfileuploadstatus" , "cppuploadform" );
+}
+
+
+
+function yamlFileInputChangedEvent(event)
+{
+ // Get the file list object
+ // > There should only be one file
+ var files = event.target.files;
+
+ // Parse the file
+ parseFileDetails( files[0] , "yamlfiledetails" );
+
+ // Upload the file
+ uploadFile( files[0] , "application/x-yaml" , "yamlfileuploadstatus" , "yamluploadform" );
+}
+
+
+
+function parseFileDetails(file,detailsID)
+{
+ document.getElementById(detailsID).innerHTML =
+ "<p>File information: <br>" +
+ "name: <strong>" + file.name + "</strong><br>" +
+ "type: <strong>" + file.type + "</strong><br>" +
+ "size: <strong>" + file.size + "</strong> bytes" +
+ "</p>";
+
+ // // display an image
+ // if (file.type.indexOf("image") == 0) {
+ // var reader = new FileReader();
+ // reader.onload = function(e) {
+ // Output(
+ // "<p><strong>" + file.name + ":</strong><br />" +
+ // '<img src="' + e.target.result + '" /></p>'
+ // );
+ // }
+ // reader.readAsDataURL(file);
+ // }
+
+ // // display text
+ // if (file.type.indexOf("text") == 0) {
+ // var reader = new FileReader();
+ // reader.onload = function(e) {
+ // Output(
+ // "<p><strong>" + file.name + ":</strong></p><pre>" +
+ // e.target.result.replace(/</g, "<").replace(/>/g, ">") +
+ // "</pre>"
+ // );
+ // }
+ // reader.readAsText(file);
+ // }
+
+}
+
+
+function uploadFile(file,expectedFileType,statusID,formID)
+{
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+
+ if (
+ xmlhttp.upload
+ &&
+ file.type == expectedFileType
+ &&
+ file.size <= document.getElementById("MAX_FILE_SIZE").value
+ )
+ {
+
+ document.getElementById(statusID).innerHTML = "Upload progress 0%";
+
+ // create progress bar
+ //var o = $id("progress");
+ //var progress = o.appendChild(document.createElement("p"));
+ //progress.appendChild(document.createTextNode("upload " + file.name));
+
+
+ // progress bar
+ xmlhttp.upload.addEventListener("progress", function(event)
+ {
+ var percent_complete = parseInt( event.loaded / event.total * 100 );
+ //progress.style.backgroundPosition = pc + "% 0";
+ document.getElementById(statusID).innerHTML = "Upload progress " + percent_complete + "%";
+ }, false);
+
+ // file received/failed
+ xmlhttp.onreadystatechange = function(event)
+ {
+ //if (xmlhttp.readyState == 4) {
+ // progress.className = (xmlhttp.status == 200 ? "success" : "failure");
+ //}
+ if (this.readyState == 4 && this.status == 200)
+ {
+ document.getElementById(statusID).innerHTML = "Upload complete." + this.responseText;
+ }
+ };
+
+ // start upload
+ xmlhttp.open("POST", "upload_async.php", true);
+ xmlhttp.setRequestHeader("X_FILENAME", file.name);
+ xmlhttp.send(file);
+
+ }
+ else
+ {
+ // Display an "ERROR" status
+ document.getElementById(statusID).innerHTML = "<strong>ERROR:</strong>";
+ // Check if upload was the cause of the error:
+ if (!(xmlhttp.upload))
+ {
+ document.getElementById(statusID).innerHTML += "<br>" + "XMLHttpRequest() does not support upload.";
+ }
+ // Check if file type is the cause of the error
+ if (!(file.type == expectedFileType))
+ {
+ document.getElementById(statusID).innerHTML += "<br>" + "file type = " + file.type + ", but it is required to be = " + expectedFileType;
+ }
+ // Check if file size is the cause of the error
+ if (!(file.size <= document.getElementById("MAX_FILE_SIZE").value))
+ {
+ document.getElementById(statusID).innerHTML += "<br>" + "file size = " + file.size + ", is greater than the max allowed of " + document.getElementById("MAX_FILE_SIZE").value + " bytes.";
+ }
+ }
+
+}
\ No newline at end of file
diff --git a/web_interface/html/js/general.js b/web_interface/html/js/general.js
new file mode 100644
index 0000000000000000000000000000000000000000..033fa60d309eb7a709f1a600160fdfc68358e324
--- /dev/null
+++ b/web_interface/html/js/general.js
@@ -0,0 +1,46 @@
+function clearLabelWithGivenID(labelID)
+{
+ document.getElementById(labelID).innerHTML = "";
+}
+
+function roundToDecimalPlaces(value,decimals)
+{
+ return Number(Math.round(value+'e'+decimals)+'e-'+decimals);
+}
+
+function incrementInputFieldWithGivenID(inputID,multiplier)
+{
+ //document.getElementById(inputID).value = 2.0;
+
+ // Get the value of the input field
+ var current_value = parseFloat( document.getElementById(inputID).value );
+
+ // Get the increment of the input field
+ var increment_amount = document.getElementById(inputID).step;
+
+ // Check that the value is a number
+ if (isNaN(current_value))
+ {
+ current_value = 0.0;
+ }
+
+ // Compute the new value
+ var new_value = roundToDecimalPlaces( current_value + multiplier * increment_amount , 3 );
+
+ // Put the new value into the field
+ document.getElementById(inputID).value = new_value;
+}
+
+function putDefaultSetpointForGivenBaseID(inputBaseID)
+{
+ // Construct the ID for the (x,y,z,yaw) input fields
+ var inputID_forX = inputBaseID + "X";
+ var inputID_forY = inputBaseID + "Y";
+ var inputID_forZ = inputBaseID + "Z";
+ var inputID_forYaw = inputBaseID + "Yaw";
+ // Set the default values
+ document.getElementById(inputID_forX).value = 0.0;
+ document.getElementById(inputID_forY).value = 0.0;
+ document.getElementById(inputID_forZ).value = 0.4;
+ document.getElementById(inputID_forYaw).value = 0.0;
+}
\ No newline at end of file
diff --git a/web_interface/html/js/getSetpointViaRosServiceCall.js b/web_interface/html/js/getSetpointViaRosServiceCall.js
new file mode 100644
index 0000000000000000000000000000000000000000..baef75df8ac98fe23a4927d38fe2ec6d20b80994
--- /dev/null
+++ b/web_interface/html/js/getSetpointViaRosServiceCall.js
@@ -0,0 +1,71 @@
+function getSetpointViaRosServiceCall_outputLabelID(controllerName, labelID)
+{
+ // Convert the "controllerName" to a heading string
+ var scriptname_for_php = "";
+ if (controllerName == "default")
+ {
+ scriptname_for_php = scriptname_for_php + "rosGetSetpointDefault";
+ }
+ else if (controllerName == "student")
+ {
+ scriptname_for_php = scriptname_for_php + "rosGetSetpointStudent";
+ }
+
+ // Set the label to be sending
+ if(labelID){document.getElementById(labelID).innerHTML = "requesting...";}
+
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+ // Add the function to be run when the response is recieved
+ xmlhttp.onreadystatechange = function()
+ {
+ if (this.readyState == 4 && this.status == 200)
+ {
+ // Get the response into a string
+ var response_as_string = this.responseText;
+ //var response_as_struct = JSON.parse(response_as_string);
+
+ // Check that the string is not empty
+ if (response_as_string)
+ {
+ // NOTE: use https://regex101.com/ to get an explaination
+ // of the regular expressions used
+
+ // Get the x value
+ //var regex_for_x = new RegExp("(x: )([-+]?\d*\.?\d*)");
+ var regex_for_x = /(x: )([-+]?\d*\.?\d*)/;
+ var found_for_x = response_as_string.match(regex_for_x);
+ var x_value_as_float = parseFloat( found_for_x[2] );
+ var x_value_as_string = x_value_as_float.toFixed(3);
+
+ // Construct and display the appropriate information
+ var base_string = "received x= ";
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + response_as_string;}
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + x_value_as_string;}
+ }
+ else
+ {
+ // Construct and display the appropriate information
+ var base_string = "received empty response";
+ if(labelID){document.getElementById(labelID).innerHTML = base_string;}
+ }
+
+ }
+ else
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ var display_message = getDisplayMessageForXMLHttpRequest(this);
+ if (this.readyState == 4)
+ {
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ else
+ {
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ }
+ };
+ xmlhttp.open("GET", "callBashScript.php?scriptname=" + scriptname_for_php, true);
+ xmlhttp.send();
+}
\ No newline at end of file
diff --git a/web_interface/html/js/roslaunch.js b/web_interface/html/js/roslaunch.js
new file mode 100644
index 0000000000000000000000000000000000000000..0f537573e430598c7f10f7d17df65169ea76f19c
--- /dev/null
+++ b/web_interface/html/js/roslaunch.js
@@ -0,0 +1,110 @@
+function roslaunch_outputLabelID_clearOtherLabels_clickOtherButtons(launchName, labelID, otherLabels, otherButtons)
+{
+ // Convert the "controllerName" to a heading string
+ var scriptname_for_php = "";
+ if (launchName == "master")
+ {
+ scriptname_for_php = "master";
+ }
+ else if (launchName == "agent")
+ {
+ scriptname_for_php = "agent";
+ }
+ else
+ {
+ return;
+ }
+
+ // Get the booleans for emulation
+ emulated_mocap = document.getElementById("checkboxEmulateMocap").checked;
+ emulated_crazyradio = document.getElementById("checkboxEmulateCrazyRadio").checked;
+
+ // Set the label to be sending
+ if(labelID){document.getElementById(labelID).innerHTML = "sending...";}
+
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+ // Add the function to be run when the response is recieved
+ xmlhttp.onreadystatechange = function()
+ {
+ if (this.readyState == 4 && this.status == 200)
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + this.responseText;}
+ // Click the "other buttons" as requested
+ if (otherButtons)
+ {
+ if ( typeof(otherButtons) == "string" )
+ {
+ document.getElementById(otherButtons).click();
+ }
+ else if (otherButtons.constructor === Array)
+ {
+ for (otherButtonID of otherButtons)
+ {
+ if (otherButtonID)
+ {
+ if ( typeof(otherButtonID) == "string" )
+ {
+ document.getElementById(otherButtonID).click();
+ }
+ }
+ }
+ }
+ }
+
+ }
+ else
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ var display_message = getDisplayMessageForXMLHttpRequest(this);
+ if (this.readyState == 4)
+ {
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ else
+ {
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ }
+ };
+ if (launchName == "master")
+ {
+ xmlhttp.open("GET", "callBashScript_roslaunch.php?scriptname=" + scriptname_for_php + "&emulatemocap=" + emulated_mocap, true);
+ xmlhttp.send();
+ }
+ else if (launchName == "agent")
+ {
+ xmlhttp.open("GET", "callBashScript_roslaunch.php?scriptname=" + scriptname_for_php + "&emulatecrazyradio=" + emulated_crazyradio, true);
+ xmlhttp.send();
+ }
+ else
+ {
+ if(labelID){document.getElementById(labelID).innerHTML = "ERROR: launch name = " + launchName + " is not a valid option";}
+ }
+
+
+ // Clear the other labels as requested
+ if (otherLabels)
+ {
+ if ( typeof(otherLabels) == "string" )
+ {
+ document.getElementById(otherLabels).innerHTML = "";
+ }
+ else if (otherLabels.constructor === Array)
+ {
+ for (otherLabelID of otherLabels)
+ {
+ if (otherLabelID)
+ {
+ if ( typeof(otherLabelID) == "string" )
+ {
+ document.getElementById(otherLabelID).innerHTML = "";
+ }
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/web_interface/html/js/sendRosMessage.js b/web_interface/html/js/sendRosMessage.js
new file mode 100644
index 0000000000000000000000000000000000000000..91bbd954e73e6fd31048ae6e88d0866ad28f043c
--- /dev/null
+++ b/web_interface/html/js/sendRosMessage.js
@@ -0,0 +1,77 @@
+function sendRosMessage_outputLabelID(rosMessage, labelID)
+{
+ // Convert the "rosMessage" to a heading string
+ var scriptname_for_php = "";
+ if (rosMessage == "connect")
+ {
+ scriptname_for_php = scriptname_for_php + "rosConnect";
+ }
+ else if (rosMessage == "disconnect")
+ {
+ scriptname_for_php = scriptname_for_php + "rosDisconnect";
+ }
+ else if (rosMessage == "takeoff")
+ {
+ scriptname_for_php = scriptname_for_php + "rosTakeoff";
+ }
+ else if (rosMessage == "land")
+ {
+ scriptname_for_php = scriptname_for_php + "rosLand";
+ }
+ else if (rosMessage == "motorsoff")
+ {
+ scriptname_for_php = scriptname_for_php + "rosMotorsoff";
+ }
+ else if (rosMessage == "enabledefault")
+ {
+ scriptname_for_php = scriptname_for_php + "rosEnabledefault";
+ }
+ else if (rosMessage == "enablestudent")
+ {
+ scriptname_for_php = scriptname_for_php + "rosEnablestudent";
+ }
+ else if (rosMessage == "loadyamldefault")
+ {
+ scriptname_for_php = scriptname_for_php + "rosLoadYamlDefault";
+ }
+ else if (rosMessage == "loadyamlstudent")
+ {
+ scriptname_for_php = scriptname_for_php + "rosLoadYamlStudent";
+ }
+
+
+
+ // Set the label to be sending
+ if(labelID){document.getElementById(labelID).innerHTML = "sending...";}
+
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+ // Add the function to be run when the response is recieved
+ xmlhttp.onreadystatechange = function()
+ {
+ if (this.readyState == 4 && this.status == 200)
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + this.responseText;}
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + "sent";}
+
+ }
+ else
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ var display_message = getDisplayMessageForXMLHttpRequest(this);
+ if (this.readyState == 4)
+ {
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ else
+ {
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ }
+ };
+ xmlhttp.open("GET", "callBashScript.php?scriptname=" + scriptname_for_php, true);
+ xmlhttp.send();
+}
\ No newline at end of file
diff --git a/web_interface/html/js/setSetpointViaRosMessage.js b/web_interface/html/js/setSetpointViaRosMessage.js
new file mode 100644
index 0000000000000000000000000000000000000000..fce3c8055e2b1cfe59a612409c0852a1aa749145
--- /dev/null
+++ b/web_interface/html/js/setSetpointViaRosMessage.js
@@ -0,0 +1,58 @@
+function setSetpointViaRosMessage_outputLabelID(controllerName, inputBaseID, labelID)
+{
+ // Convert the "controllerName" to a heading string
+ var scriptname_for_php = "";
+ if (controllerName == "default")
+ {
+ scriptname_for_php = scriptname_for_php + "rosSetSetpointDefault";
+ }
+ else if (controllerName == "student")
+ {
+ scriptname_for_php = scriptname_for_php + "rosSetSetpointStudent";
+ }
+
+ // Get the (x,y,z,yaw) values of the new setpoint
+ var inputID_forX = inputBaseID + "X";
+ var inputID_forY = inputBaseID + "Y";
+ var inputID_forZ = inputBaseID + "Z";
+ var inputID_forYaw = inputBaseID + "Yaw";
+ x_new = document.getElementById(inputID_forX).value;
+ y_new = document.getElementById(inputID_forY).value;
+ z_new = document.getElementById(inputID_forZ).value;
+ yaw_new = document.getElementById(inputID_forYaw).value / 57.29578;
+
+
+ // Set the label to be sending
+ if(labelID){document.getElementById(labelID).innerHTML = "sending...";}
+
+ // Create a variable for sending an AJAX request
+ var xmlhttp = new XMLHttpRequest();
+ // Add the function to be run when the response is recieved
+ xmlhttp.onreadystatechange = function()
+ {
+ if (this.readyState == 4 && this.status == 200)
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + this.responseText;}
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + "sent";}
+
+ }
+ else
+ {
+ // Construct and display the appropriate information
+ var base_string = "";
+ var display_message = getDisplayMessageForXMLHttpRequest(this);
+ if (this.readyState == 4)
+ {
+ if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ else
+ {
+ //if(labelID){document.getElementById(labelID).innerHTML = base_string + display_message;}
+ }
+ }
+ };
+ xmlhttp.open("GET", "callBashScript_setNewSetpoint.php?scriptname=" + scriptname_for_php + "&x=" + x_new + "&y=" + y_new + "&z=" + z_new + "&yaw=" + yaw_new, true);
+ xmlhttp.send();
+}
\ No newline at end of file
diff --git a/web_interface/html/js/sse_agentStatus.js b/web_interface/html/js/sse_agentStatus.js
new file mode 100644
index 0000000000000000000000000000000000000000..34c1172d463b85763d2f5512174812fbeb2f47d7
--- /dev/null
+++ b/web_interface/html/js/sse_agentStatus.js
@@ -0,0 +1,468 @@
+let agentStatusEventSource;
+
+window.onload = function() {
+ document.getElementById("checkboxTopBarStatus").checked = false;
+};
+
+function checkboxTopBarStatus_changed()
+{
+ if ( document.getElementById("checkboxTopBarStatus").checked )
+ {
+ severSentAgentStatus_start();
+ }
+ else
+ {
+ severSentAgentStatus_stop();
+ }
+}
+
+
+function severSentAgentStatus_start()
+{
+ // Check is server sent events are supported
+ if (typeof(EventSource) == "undefined")
+ {
+ // Server-sent events are NOT supported
+ // Display this to the user
+ document.getElementById("agentStatusDebuggingDiv").innerHTML = "Server Sent Events are not supported by this browser.";
+ return;
+ }
+
+ // If the code makes it to here, then
+ // server sent events are supported
+
+ // Create the "EventSource" variable
+ agentStatusEventSource = new EventSource("sse_agentStatus.php");
+
+ // Handle the "onopen" event
+ agentStatusEventSource.onopen = function(eventSource)
+ {
+ // DEBUGGING: print out
+ //document.getElementById("agentStatusDebuggingDiv").innerHTML += "Event: on open <br>";
+ };
+
+ // Handle the "onerror" event
+ agentStatusEventSource.onerror = function(eventSource)
+ {
+ // DEBUGGING: print out
+ document.getElementById("agentStatusDebuggingDiv").innerHTML += "Event: on error <br>";
+
+ // Check is the event is still connecting
+ if (this.readyState == EventSource.CONNECTING)
+ {
+ // DEBUGGING: print out
+ document.getElementById("agentStatusDebuggingDiv").innerHTML += "Reconnecting (readyState = " + this.readyState + ")... <br>";
+ }
+ else
+ {
+ // DEBUGGING: print out
+ document.getElementById("agentStatusDebuggingDiv").innerHTML += "Error has occured. <br>";
+ }
+ };
+
+ // Handle the "message" event
+ //agentStatusEventSource.onmessage = function(eventSource)
+ agentStatusEventSource.addEventListener("message", function(eventSource)
+ {
+ // DEBUGGING: print out the whole message
+ //document.getElementById("agentStatusDebuggingDiv").innerHTML += "Event: message, data: " + eventSource.data + " <br>";
+ });
+
+ // Handle the "ping" event
+ agentStatusEventSource.addEventListener("agentstatus", function(eventSource)
+ {
+ // Extract "agent found" entry in the json
+ agent_found = JSON.parse(eventSource.data).agentfound;
+
+ // If the agent is found
+ if ( agent_found === "true" )
+ {
+ // Parse the status dictionary from the JSON
+ status_dict = JSON.parse(eventSource.data).statusjson;
+
+ // Call the fuctions to update the status icons
+ // > For the Radio status:
+ updateStatusIconRadio( status_dict.crazyradiostatus );
+ // > For the Battery level:
+ updateStatusIconBattery( status_dict.batterylevel );
+ // > For the Flying state:
+ updateStatusIconFlyingState( status_dict.flyingstate );
+ // > For the Instant controller:
+ updateStatusForInstantController( status_dict.instantcontroller );
+
+ // Call the function to update the "measurement,
+ // setpoint, error" tables
+ // > For the Default controller:
+ updateDefaultMseTable( status_dict.stateestimate , status_dict.setpointdefault );
+ // > For the Student controller:
+ updateStudentMseTable( status_dict.stateestimate , status_dict.setpointstudent );
+
+ // Call the function to update the "debug values" table
+ updateStudentDebugValuesTable( status_dict.debugvaluesstudent );
+
+ // DEBUGGING: print out the whole message
+ //document.getElementById("agentStatusDebuggingDiv").innerHTML += "Event: agent status, data: " + eventSource.data + " <br>";
+ }
+ // Otherwise, the agent is NOT found
+ else
+ {
+ // So set all the icons accordingly
+ // > For the Radio status:
+ updateStatusIconRadio( "disconnected" );
+ // > For the Battery level:
+ updateStatusIconBattery( "unavailable" );
+ // > For the Flying state:
+ updateStatusIconFlyingState( "unavailable" );
+ // > For the Instant controller:
+ updateStatusForInstantController( "none" );
+
+ // Call the function to clear the "measurement,
+ // setpoint, error" tables
+ // > For the Default controller:
+ clearDefaultMseTable();
+ // > For the Student controller:
+ clearStudentMseTable();
+
+ // Call the function to clear the "debug values" table
+ clearStudentDebugValuesTable();
+
+ // DEBUGGING: print out the whole message
+ //document.getElementById("agentStatusDebuggingDiv").innerHTML += "Event: agent status, data: " + eventSource.data + " <br>";
+ }
+ });
+
+} // END OF: function severSentAgentStatus_start()
+
+
+
+
+
+function severSentAgentStatus_stop()
+{
+ // Check is server sent events are supported
+ if (typeof(EventSource) == "undefined")
+ {
+ // Server-sent events are NOT supported
+ // Display this to the user
+ document.getElementById("agentStatusDebuggingDiv").innerHTML = "Server Sent Events are not supported by this browser.";
+ return;
+ }
+
+ // If the code makes it to here, then
+ // server sent events are supported
+
+ // Close the "EventSource" variable
+ agentStatusEventSource.close();
+
+ // DEBUGGING: print out
+ //document.getElementById("agentStatusDebuggingDiv").innerHTML += "Closed. <br>";
+
+ // So set all the icons accordingly
+ // > For the Radio status:
+ updateStatusIconRadio( "disconnected" );
+ // > For the Battery level:
+ updateStatusIconBattery( "unavailable" );
+ // > For the Flying state:
+ updateStatusIconFlyingState( "unavailable" );
+ // > For the Instant controller:
+ updateStatusForInstantController( "none" );
+
+ // Call the function to clear the "measurement,
+ // setpoint, error" tables
+ // > For the Default controller:
+ clearDefaultMseTable();
+ // > For the Student controller:
+ clearStudentMseTable();
+
+ // Call the function to clear the "debug values" table
+ clearStudentDebugValuesTable();
+
+} // END OF: function severSentAgentStatus_stop()
+
+
+
+function updateStatusIconRadio( status_string )
+{
+ if ( typeof status_string !== "string" )
+ {
+ // Default to disconnected
+ status_string = "disconnected";
+ }
+
+ switch ( status_string.toLowerCase() )
+ {
+ case "connected":
+ document.getElementById("radio-icon").src = "img/rf_connected.png";
+ break;
+ case "connecting":
+ document.getElementById("radio-icon").src = "img/rf_connecting.png";
+ break;
+ case "disconnected":
+ document.getElementById("radio-icon").src = "img/rf_disconnected.png";
+ break;
+ default:
+ document.getElementById("radio-icon").src = "img/rf_disconnected.png";
+ }
+
+} // END OF: "function updateStatusIconRadio( status_string )"
+
+
+
+function updateStatusIconBattery( level_string )
+{
+ if ( typeof level_string !== "string" )
+ {
+ // Default to unavailable
+ level_string = "unavailable";
+ }
+
+ switch ( level_string.toLowerCase() )
+ {
+ case "000":
+ document.getElementById("battery-icon").src = "img/battery_empty.png";
+ break;
+ case "010":
+ document.getElementById("battery-icon").src = "img/battery_20.png";
+ break;
+ case "020":
+ document.getElementById("battery-icon").src = "img/battery_20.png";
+ break;
+ case "030":
+ document.getElementById("battery-icon").src = "img/battery_40.png";
+ break;
+ case "040":
+ document.getElementById("battery-icon").src = "img/battery_40.png";
+ break;
+ case "050":
+ document.getElementById("battery-icon").src = "img/battery_60.png";
+ break;
+ case "060":
+ document.getElementById("battery-icon").src = "img/battery_60.png";
+ break;
+ case "070":
+ document.getElementById("battery-icon").src = "img/battery_80.png";
+ break;
+ case "080":
+ document.getElementById("battery-icon").src = "img/battery_80.png";
+ break;
+ case "090":
+ document.getElementById("battery-icon").src = "img/battery_full.png";
+ break;
+ case "100":
+ document.getElementById("battery-icon").src = "img/battery_full.png";
+ break;
+ case "unknown":
+ document.getElementById("battery-icon").src = "img/battery_unknown.png";
+ break;
+ case "unavailable":
+ document.getElementById("battery-icon").src = "img/battery_unavailable.png";
+ break;
+ default:
+ document.getElementById("battery-icon").src = "img/battery_unavailable.png";
+ }
+
+} // END OF: "function updateStatusIconBattery( level_string )"
+
+
+
+function updateStatusIconFlyingState( state_string )
+{
+ if ( typeof state_string !== "string" )
+ {
+ // Default to unavailable
+ state_string = "unavailable";
+ }
+
+ switch ( state_string.toLowerCase() )
+ {
+ case "motorsoff":
+ document.getElementById("flying-state-icon").src = "img/flying_state_off.png";
+ break;
+ case "takeoff":
+ document.getElementById("flying-state-icon").src = "img/flying_state_enabling.png";
+ break;
+ case "flying":
+ document.getElementById("flying-state-icon").src = "img/flying_state_flying.png";
+ break;
+ case "land":
+ document.getElementById("flying-state-icon").src = "img/flying_state_disabling.png";
+ break;
+ case "unavailable":
+ document.getElementById("flying-state-icon").src = "img/flying_state_unavailable.png";
+ break;
+ default:
+ document.getElementById("flying-state-icon").src = "img/flying_state_unavailable.png";
+ }
+
+} // END OF: "function updateStatusIconRadio( status_string )"
+
+
+
+function updateStatusForInstantController( controller_string )
+{
+ if ( typeof controller_string !== "string" )
+ {
+ // Default to unavailable
+ controller_string = "none";
+ }
+
+ // Colours:
+ // #c43c35 Dark Red
+ // #f44336 Light Red
+ // #57a957 Green
+
+ // Set all border colours to red
+ document.getElementById("control-tab-panel-default").style.borderColor = "#c43c35";
+ document.getElementById("control-tab-panel-student").style.borderColor = "#c43c35";
+
+ switch ( controller_string.toLowerCase() )
+ {
+ case "default":
+ document.getElementById("control-tab-panel-default").style.borderColor = "#57a957";
+ break;
+ case "student":
+ document.getElementById("control-tab-panel-student").style.borderColor = "#57a957";
+ break;
+ }
+
+} // END OF: "function updateStatusIconRadio( status_string )"
+
+
+function updateDefaultMseTable( measurement_dict , setpoint_dict )
+{
+ if ( (typeof measurement_dict !== "object") || (typeof setpoint_dict !== "object") )
+ {
+ // Call function to set fields to blank
+ clearDefaultMseTable();
+ return;
+ }
+
+ document.getElementById("default-measurement-x").innerHTML = measurement_dict.x.toFixed(3);
+ document.getElementById("default-measurement-y").innerHTML = measurement_dict.y.toFixed(3);
+ document.getElementById("default-measurement-z").innerHTML = measurement_dict.z.toFixed(3);
+ document.getElementById("default-measurement-yaw").innerHTML = measurement_dict.yaw.toFixed(3);
+ document.getElementById("default-measurement-pitch").innerHTML = measurement_dict.pitch.toFixed(1);
+ document.getElementById("default-measurement-roll").innerHTML = measurement_dict.roll.toFixed(1);
+
+ document.getElementById("default-setpoint-x").innerHTML = setpoint_dict.x.toFixed(3);
+ document.getElementById("default-setpoint-y").innerHTML = setpoint_dict.y.toFixed(3);
+ document.getElementById("default-setpoint-z").innerHTML = setpoint_dict.z.toFixed(3);
+ document.getElementById("default-setpoint-yaw").innerHTML = setpoint_dict.yaw.toFixed(1);
+
+ document.getElementById("default-error-x").innerHTML = (measurement_dict.x -setpoint_dict.x ).toFixed(3);
+ document.getElementById("default-error-y").innerHTML = (measurement_dict.y -setpoint_dict.y ).toFixed(3);
+ document.getElementById("default-error-z").innerHTML = (measurement_dict.z -setpoint_dict.z ).toFixed(3);
+ document.getElementById("default-error-yaw").innerHTML = (measurement_dict.yaw-setpoint_dict.yaw).toFixed(1);
+
+} // END OF: "function updateDefaultMseTable( measurement_dict , setpoint_dict )"
+
+function clearDefaultMseTable()
+{
+ document.getElementById("default-measurement-x").innerHTML = "xx.xx";
+ document.getElementById("default-measurement-y").innerHTML = "xx.xx";
+ document.getElementById("default-measurement-z").innerHTML = "xx.xx";
+ document.getElementById("default-measurement-yaw").innerHTML = "xx.xx";
+ document.getElementById("default-measurement-pitch").innerHTML = "xx.xx";
+ document.getElementById("default-measurement-roll").innerHTML = "xx.xx";
+
+ document.getElementById("default-setpoint-x").innerHTML = "xx.xx";
+ document.getElementById("default-setpoint-y").innerHTML = "xx.xx";
+ document.getElementById("default-setpoint-z").innerHTML = "xx.xx";
+ document.getElementById("default-setpoint-yaw").innerHTML = "xx.xx";
+
+ document.getElementById("default-error-x").innerHTML = "xx.xx";
+ document.getElementById("default-error-y").innerHTML = "xx.xx";
+ document.getElementById("default-error-z").innerHTML = "xx.xx";
+ document.getElementById("default-error-yaw").innerHTML = "xx.xx";
+
+} // END OF: "function clearDefaultMseTable()"
+
+
+
+function updateStudentMseTable( measurement_dict , setpoint_dict )
+{
+ if ( (typeof measurement_dict !== "object") || (typeof setpoint_dict !== "object") )
+ {
+ // Call function to set fields to blank
+ clearStudentMseTable();
+ return;
+ }
+
+ document.getElementById("student-measurement-x").innerHTML = measurement_dict.x.toFixed(3);
+ document.getElementById("student-measurement-y").innerHTML = measurement_dict.y.toFixed(3);
+ document.getElementById("student-measurement-z").innerHTML = measurement_dict.z.toFixed(3);
+ document.getElementById("student-measurement-yaw").innerHTML = measurement_dict.yaw.toFixed(3);
+ document.getElementById("student-measurement-pitch").innerHTML = measurement_dict.pitch.toFixed(1);
+ document.getElementById("student-measurement-roll").innerHTML = measurement_dict.roll.toFixed(1);
+
+ document.getElementById("student-setpoint-x").innerHTML = setpoint_dict.x.toFixed(3);
+ document.getElementById("student-setpoint-y").innerHTML = setpoint_dict.y.toFixed(3);
+ document.getElementById("student-setpoint-z").innerHTML = setpoint_dict.z.toFixed(3);
+ document.getElementById("student-setpoint-yaw").innerHTML = setpoint_dict.yaw.toFixed(1);
+
+ document.getElementById("student-error-x").innerHTML = (measurement_dict.x -setpoint_dict.x ).toFixed(3);
+ document.getElementById("student-error-y").innerHTML = (measurement_dict.y -setpoint_dict.y ).toFixed(3);
+ document.getElementById("student-error-z").innerHTML = (measurement_dict.z -setpoint_dict.z ).toFixed(3);
+ document.getElementById("student-error-yaw").innerHTML = (measurement_dict.yaw-setpoint_dict.yaw).toFixed(1);
+
+} // END OF: "function updateStudentMseTable( measurement_dict , setpoint_dict )"
+
+function clearStudentMseTable()
+{
+ document.getElementById("student-measurement-x").innerHTML = "xx.xx";
+ document.getElementById("student-measurement-y").innerHTML = "xx.xx";
+ document.getElementById("student-measurement-z").innerHTML = "xx.xx";
+ document.getElementById("student-measurement-yaw").innerHTML = "xx.xx";
+ document.getElementById("student-measurement-pitch").innerHTML = "xx.xx";
+ document.getElementById("student-measurement-roll").innerHTML = "xx.xx";
+
+ document.getElementById("student-setpoint-x").innerHTML = "xx.xx";
+ document.getElementById("student-setpoint-y").innerHTML = "xx.xx";
+ document.getElementById("student-setpoint-z").innerHTML = "xx.xx";
+ document.getElementById("student-setpoint-yaw").innerHTML = "xx.xx";
+
+ document.getElementById("student-error-x").innerHTML = "xx.xx";
+ document.getElementById("student-error-y").innerHTML = "xx.xx";
+ document.getElementById("student-error-z").innerHTML = "xx.xx";
+ document.getElementById("student-error-yaw").innerHTML = "xx.xx";
+
+} // END OF: "function clearStudentMseTable()"
+
+
+
+
+function updateStudentDebugValuesTable( values_dict )
+{
+ if (typeof values_dict !== "object")
+ {
+ // Call function to set fields to blank
+ clearStudentDebugValuesTable();
+ return;
+ }
+
+ document.getElementById("debug-value1").innerHTML = values_dict.value1.toFixed(3);
+ document.getElementById("debug-value2").innerHTML = values_dict.value2.toFixed(3);
+ document.getElementById("debug-value3").innerHTML = values_dict.value3.toFixed(3);
+ document.getElementById("debug-value4").innerHTML = values_dict.value4.toFixed(3);
+ document.getElementById("debug-value5").innerHTML = values_dict.value5.toFixed(3);
+ document.getElementById("debug-value6").innerHTML = values_dict.value6.toFixed(3);
+ document.getElementById("debug-value7").innerHTML = values_dict.value7.toFixed(3);
+ document.getElementById("debug-value8").innerHTML = values_dict.value8.toFixed(3);
+ document.getElementById("debug-value9").innerHTML = values_dict.value9.toFixed(3);
+ document.getElementById("debug-value10").innerHTML = values_dict.value10.toFixed(3);
+
+} // END OF: "function updateStudentDebugValuesTable( values_dict )"
+
+function clearStudentDebugValuesTable()
+{
+ document.getElementById("debug-value1").innerHTML = "xx.xx";
+ document.getElementById("debug-value2").innerHTML = "xx.xx";
+ document.getElementById("debug-value3").innerHTML = "xx.xx";
+ document.getElementById("debug-value4").innerHTML = "xx.xx";
+ document.getElementById("debug-value5").innerHTML = "xx.xx";
+ document.getElementById("debug-value6").innerHTML = "xx.xx";
+ document.getElementById("debug-value7").innerHTML = "xx.xx";
+ document.getElementById("debug-value8").innerHTML = "xx.xx";
+ document.getElementById("debug-value9").innerHTML = "xx.xx";
+ document.getElementById("debug-value10").innerHTML = "xx.xx";
+} // END OF: "function clearStudentDebugValuesTable()"
\ No newline at end of file
diff --git a/web_interface/html/page_footer.html b/web_interface/html/page_footer.html
new file mode 100644
index 0000000000000000000000000000000000000000..62d09b8221fb59ebb772816f5ff6f9976095615a
--- /dev/null
+++ b/web_interface/html/page_footer.html
@@ -0,0 +1 @@
+</html>
\ No newline at end of file
diff --git a/web_interface/html/page_header.html b/web_interface/html/page_header.html
new file mode 100644
index 0000000000000000000000000000000000000000..92c815de57e633259c3d13f5dde29ac9a0bf94e4
--- /dev/null
+++ b/web_interface/html/page_header.html
@@ -0,0 +1,43 @@
+<!DOCTYPE html>
+<html lang="en">
+
+<head>
+
+ <!-- Some technical meta data -->
+ <meta charset="UTF-8" />
+ <meta http-equiv="X-UA-Compatible" content="IE=edge,chrome=1">
+ <meta name="viewport" content="width=device-width, initial-scale=1.0">
+
+ <!-- META DATA ABOUT ME AND MY PAGE -->
+ <title>D-FaLL Web Interface</title>
+
+ <meta name="description" content="Web interface for controlling the D-FaLL-System" />
+
+ <meta name="author" content="Paul N. Beuchat">
+
+ <meta name="keywords" content="ethz, D-ITET, IfA, dfall, D-FaLL" />
+
+ <!-- Link the CSS files -->
+ <link rel="stylesheet" type="text/css" href="css/default.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/layout.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/body.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/buttons.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/switches.css?ver=0.1" />
+
+ <link rel="stylesheet" type="text/css" href="css/tabs_main.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/tabs_home.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/tabs_control.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/status_bar.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/measurement_setpoint_error_table.css?ver=0.1" />
+ <link rel="stylesheet" type="text/css" href="css/debug_values_table.css?ver=0.1" />
+
+ <!-- Link the JavaScript files -->
+ <script src="js/general.js?ver=0.1"></script>
+ <script src="js/callbashscript.js?ver=0.1"></script>
+ <script src="js/callgit.js?ver=0.1"></script>
+ <script src="js/roslaunch.js?ver=0.1"></script>
+ <script src="js/sendRosMessage.js?ver=0.1"></script>
+ <script src="js/setSetpointViaRosMessage.js?ver=0.1"></script>
+ <script src="js/getSetpointViaRosServiceCall.js?ver=0.1"></script>
+
+</head>
\ No newline at end of file
diff --git a/web_interface/html/phpinfo.php b/web_interface/html/phpinfo.php
new file mode 100644
index 0000000000000000000000000000000000000000..54bd1bc952e8c41d31c41cd53d4345b8e58ed466
--- /dev/null
+++ b/web_interface/html/phpinfo.php
@@ -0,0 +1 @@
+<?php echo 'Simple test for now'; ?>
diff --git a/web_interface/html/sse_agentStatus.php b/web_interface/html/sse_agentStatus.php
new file mode 100644
index 0000000000000000000000000000000000000000..60eeed050c0c046e9bd82d49ff6cc8b688679296
--- /dev/null
+++ b/web_interface/html/sse_agentStatus.php
@@ -0,0 +1,30 @@
+ <?php
+ //date_default_timezone_set("America/New_York");
+ header("Cache-Control: no-cache");
+ header("Content-Type: text/event-stream");
+ //header('Connection: keep-alive');
+
+ $event_id_counter = 0;
+
+ while (true) {
+ // Every second, send a "ping" event.
+
+ echo "event: agentstatus\n";
+
+ $output = shell_exec("./bashscripts/rosGetStatusJson_forAgent.sh " . $event_id_counter);
+
+ //$curDate = date(DATE_ISO8601);
+ //echo 'data: {"time": "' . $curDate . '"}';
+
+ echo "data: $output";
+ echo "\n\n";
+
+ // Send a simple message at random intervals.
+
+ $event_id_counter++;
+
+ ob_end_flush();
+ flush();
+ usleep(250000);
+ }
+?>
\ No newline at end of file
diff --git a/web_interface/html/updatewebinterface.php b/web_interface/html/updatewebinterface.php
new file mode 100644
index 0000000000000000000000000000000000000000..663b75acb59c2ae4738bf642eb453ba449f278b4
--- /dev/null
+++ b/web_interface/html/updatewebinterface.php
@@ -0,0 +1,5 @@
+<?php
+ $temp = shell_exec("./bashscripts/updatewebinterface.sh");
+ $output = "<pre>$temp</pre>";
+ echo "$output";
+?>
diff --git a/web_interface/html/upload.php b/web_interface/html/upload.php
new file mode 100644
index 0000000000000000000000000000000000000000..0daebd7cce2ac346d35c8c0ae6dd8c20b0cb6861
--- /dev/null
+++ b/web_interface/html/upload.php
@@ -0,0 +1,22 @@
+<?php
+$target_dir = "uploads/";
+$target_file = $target_dir . basename($_FILES["fileToUpload"]["name"]);
+$uploadOk = 1;
+$imageFileType = strtolower(pathinfo($target_file,PATHINFO_EXTENSION));
+// Allow certain file formats
+if($imageFileType != "cpp") {
+ echo "Sorry, only C++ files are allowed. ";
+ $uploadOk = 0;
+}
+// Check if $uploadOk is set to 0 by an error
+if ($uploadOk == 0) {
+ echo "Your file was not uploaded.";
+// if everything is ok, try to upload file
+} else {
+ if (move_uploaded_file($_FILES["fileToUpload"]["tmp_name"], $target_file)) {
+ echo "The file ". basename( $_FILES["fileToUpload"]["name"]). " has been uploaded.";
+ } else {
+ echo "Sorry, there was an error uploading your file.";
+ }
+}
+?>
\ No newline at end of file
diff --git a/web_interface/html/upload_async.php b/web_interface/html/upload_async.php
new file mode 100644
index 0000000000000000000000000000000000000000..324e92e621b0fc6d2e5c49f2fcf0e2ca2d5fe718
--- /dev/null
+++ b/web_interface/html/upload_async.php
@@ -0,0 +1,41 @@
+<?php
+
+ echo "DEBUG1";
+
+ $fn = (isset($_SERVER['HTTP_X_FILENAME']) ? $_SERVER['HTTP_X_FILENAME'] : false);
+
+ echo "DEBUG2";
+
+ echo "$fn";
+
+ if ($fn)
+ {
+ echo "DEBUG3";
+
+ // AJAX call
+ file_put_contents(
+ 'uploads/' . $fn,
+ file_get_contents('php://input')
+ );
+ echo "$fn uploaded";
+ exit();
+
+ }
+ else
+ {
+ echo "DEBUG4";
+ // // form submit
+ // $files = $_FILES['fileselect'];
+
+ // foreach ($files['error'] as $id => $err) {
+ // if ($err == UPLOAD_ERR_OK) {
+ // $fn = $files['name'][$id];
+ // move_uploaded_file(
+ // $files['tmp_name'][$id],
+ // 'uploads/' . $fn
+ // );
+ // echo "<p>File $fn uploaded.</p>";
+ // }
+ // }
+ }
+?>
\ No newline at end of file
diff --git a/web_interface/install_dfall_server_WIP.sh b/web_interface/install_dfall_server_WIP.sh
new file mode 100644
index 0000000000000000000000000000000000000000..a0aea03a682aa50494753963850817af99bde4cd
--- /dev/null
+++ b/web_interface/install_dfall_server_WIP.sh
@@ -0,0 +1,59 @@
+# Install the apache web server
+sudo apt install apache2
+
+# Install php
+sudo apt install php
+
+# DISABLE iPv6
+
+# CRTICAL TO GET THE gateway4 and nameserver correct
+# for example 192.168.0.1
+
+# Create a shared folder
+cd /home
+sudo mkdir www-share
+sudo chmod 777 www-share
+cd www-share
+sudo -u www-data mkdir dfall
+sudo -u www-data chmod 775 dfall
+
+# Clone the repository
+# NOTE: very important here is that the repository
+# is cloned as the www-data user
+cd /home/www-share/dfall
+sudo -u www-data git clone https://gitlab.ethz.ch/dfall/dfall-system
+
+# Add the necessary line to the "/etc/sudoers" file
+# that allows the "www-data" user to execute
+# "git pull" commands
+# >> www-data ALL=(www-data) /usr/bin/git pull
+
+# Add the "www-data" user to the "plugdev" group
+# NOTE: this is the group nominated in the udev
+# rules for the CrazyRadio (see the "install"
+# folder). This allows a CrazyRadio node
+# that is launched by the web interface to
+# access the USB dongle.
+sudo usermod -a -G plugdev www-data
+
+# To confirm the group allocation, view the file:
+# >> less /etc/group
+
+# If you need to remove a user from a group, then:
+# SYNTAX: >> deluser <username> <groupname>
+# >> deluser www-data plugdev
+
+
+
+
+# ============================================ #
+# USEFUL COMMANDS
+#
+# The apache web server can be
+# {stop,start,restart,reload}
+# using the "systemctl" command as follows:
+#
+#sudo systemctl stop apache2.service
+#sudo systemctl start apache2.service
+#sudo systemctl restart apache2.service
+#sudo systemctl reload apache2.service
\ No newline at end of file