From 9cfd6dedfc9b10ef2a5c10358239ce85523f115c Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 13 Apr 2020 17:01:04 +0200
Subject: [PATCH] Extended the FlyingVehicleState message definition to inlude
a type, and velocities (both translational and angular). Update the Vicon and
Qualisys publishers to fill in the type. Updated the CrazyRadio node to fill
in this expanded FlyingVehicleState correctly, including adding a crazyflie
name property to the CrazyRadioClient class. Removed all empty lines as end
of message definitions.
---
.../flyingAgentGUI/src/coordinatorrow.cpp | 2 +-
.../GUI_Qt/flyingAgentGUI/src/topbanner.cpp | 2 +-
.../src/dfall_pkg/crazyradio/CrazyRadio.py | 65 +++++++++++++++----
dfall_ws/src/dfall_pkg/crazyradio/TestCF.py | 2 +-
.../src/dfall_pkg/include/nodes/Constants.h | 19 +++++-
.../include/nodes/DefaultControllerService.h | 2 +-
dfall_ws/src/dfall_pkg/msg/AreaBounds.msg | 3 +-
dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg | 3 +-
dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg | 3 +-
.../src/dfall_pkg/msg/FlyingVehicleState.msg | 7 ++
dfall_ws/src/dfall_pkg/msg/Setpoint.msg | 2 +-
.../src/dfall_pkg/msg/UnlabeledMarker.msg | 2 +-
.../dfall_pkg/src/nodes/FlyingAgentClient.cpp | 3 +-
.../src/dfall_pkg/src/nodes/MocapEmulator.cpp | 2 +
.../src/nodes/ViconDataPublisher.cpp | 51 ++++++++++-----
.../src/qualisys/QualisysDataPublisher.py | 8 +++
16 files changed, 133 insertions(+), 43 deletions(-)
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 eb55555b..a93a3d8f 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/topbanner.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/topbanner.cpp
index baa47e7b..b2d401b7 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/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index 6828e3df..2a9ae6dc 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -44,7 +44,7 @@ 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 CrazyflieStateEstimate
+from dfall_pkg.msg import FlyingVehicleState
from dfall_pkg.msg import GyroMeasurements
# Import dfall_pkg services
from dfall_pkg.srv import IntIntService
@@ -124,6 +124,14 @@ CMD_CRAZYFLY_TAKE_OFF = 11
CMD_CRAZYFLY_LAND = 12
CMD_CRAZYFLY_MOTORS_OFF = 13
+# 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 logging
rp = RosPack()
record_file = rp.get_path('dfall_pkg') + '/LoggingOnboard.bag'
@@ -139,12 +147,24 @@ class CrazyRadioClient:
"""
def __init__(self):
- # Setpoints to be sent to the CrazyFlie
- self._status = DISCONNECTED
- self.link_uri = ""
-
+ # 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 = ""
+ # > For the name of the connected crazyflie
+ self.crazyflie_name = ""
+
+ # > 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
@@ -173,7 +193,10 @@ class CrazyRadioClient:
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
@@ -217,8 +240,8 @@ class CrazyRadioClient:
def _data_received_stateEstimate_callback(self, timestamp, data, logconf):
- # Initialise the variable for the state estimate data
- cfStateEstimate = CrazyflieStateEstimate()
+ # Initialise the variable for the flying vehicle state
+ cfStateEstimate = FlyingVehicleState()
# Retrieve the values from the data packet received
# > (x,y,z) positions
@@ -245,10 +268,22 @@ class CrazyRadioClient:
# 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()
@@ -258,6 +293,9 @@ class CrazyRadioClient:
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 )
@@ -630,7 +668,12 @@ 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)
+
+ # 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 the following lines of code to connect without data
@@ -659,7 +702,7 @@ if __name__ == '__main__':
# Initialise a publisher for the state estimate
global cfstateEstimate_pub
- cfstateEstimate_pub = rospy.Publisher(node_name + '/CFStateEstimate', CrazyflieStateEstimate, queue_size=10)
+ cfstateEstimate_pub = rospy.Publisher(node_name + '/CFStateEstimate', FlyingVehicleState, queue_size=10)
diff --git a/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py b/dfall_ws/src/dfall_pkg/crazyradio/TestCF.py
index 9598f852..0f3431c9 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/Constants.h b/dfall_ws/src/dfall_pkg/include/nodes/Constants.h
index 94b022d7..4bd6a094 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
@@ -121,6 +118,21 @@
+// ----------------------------------------------------------------------------------
+//
+//
+//
+//
+//
+// ----------------------------------------------------------------------------------
+
+// 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
+
+
@@ -185,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/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index a838f2f2..92077899 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"
diff --git a/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg b/dfall_ws/src/dfall_pkg/msg/AreaBounds.msg
index b367b15d..5b82cabb 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/CrazyflieDB.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieDB.msg
index a65dc00e..28ffc012 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/CrazyflieEntry.msg b/dfall_ws/src/dfall_pkg/msg/CrazyflieEntry.msg
index 1bf527e2..b8d7fd54 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
index e5c00c0a..1e0b1c4e 100644
--- a/dfall_ws/src/dfall_pkg/msg/FlyingVehicleState.msg
+++ b/dfall_ws/src/dfall_pkg/msg/FlyingVehicleState.msg
@@ -1,9 +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/Setpoint.msg b/dfall_ws/src/dfall_pkg/msg/Setpoint.msg
index b3f94d9f..eb02bd5d 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 c05b0c57..a0b6cea0 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/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 553e6e56..9e249f43 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -1230,7 +1230,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
{
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
index ab75c383..ceca93a8 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
@@ -86,6 +86,8 @@ void timerCallback_mocap_publisher(const ros::TimerEvent&)
FlyingVehicleState quadrotor_data;
// Fill in the details
+ // > For the type
+ quadrotor_data.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
// > For the name
quadrotor_data.vehicleName = it->get_id_string();
// > For the occulsion flag
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp b/dfall_ws/src/dfall_pkg/src/nodes/ViconDataPublisher.cpp
index 7a5fe429..9d2a762c 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
- FlyingVehicleState cfData;
- cfData.vehicleName = subjectName;
-
- cfData.isValid = 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);
}
@@ -327,7 +344,9 @@ void testFakeData(ros::Publisher viconDataPublisher)
// TODO: Fake CF data
FlyingVehicleState crazyfly;
- crazyfly.isValid = false;
+ crazyfly.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
+
+ crazyfly.isValid = true;
crazyfly.vehicleName = "CF1";
crazyfly.x = 0;
diff --git a/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py b/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py
index cea66f2a..043d9461 100755
--- a/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py
+++ b/dfall_ws/src/dfall_pkg/src/qualisys/QualisysDataPublisher.py
@@ -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()
@@ -281,6 +286,9 @@ class QualisysQtmClient:
# > 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.vehicleName = self._list_of_body_names[current_body_index]
--
GitLab