From 1b87a73f8d11c8b26b02138249939df8e22b9d8c Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 20 Apr 2020 16:33:06 +0200
Subject: [PATCH] Connected the CrazyRadioEmulator to its Quadrotor Simulator.
Tested to be working, however, it is important that to not emulate both Mocap
and Crazyradio at the same time. Next step is to add a visualization of the
simulator to the System Config GUI.
---
.../flyingAgentGUI/include/controllertabs.h | 14 +-
.../flyingAgentGUI/src/controllertabs.cpp | 44 +++++-
.../src/dfall_pkg/crazyradio/CrazyRadio.py | 12 +-
.../include/nodes/CrazyRadioEmulator.h | 13 +-
.../src/dfall_pkg/param/CrazyRadioConfig.yaml | 15 ++-
.../src/classes/QuadrotorSimulator.cpp | 6 +
.../src/nodes/CrazyRadioEmulator.cpp | 70 ++++++++--
.../src/nodes/DefaultControllerService.cpp | 127 +++++++++++-------
.../dfall_pkg/src/nodes/FlyingAgentClient.cpp | 42 +++---
9 files changed, 243 insertions(+), 100 deletions(-)
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 ec3e39c1..c8bbfba7 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
@@ -157,7 +157,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 +173,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/src/controllertabs.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/controllertabs.cpp
index fe2cf599..10fa29b6 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
@@ -209,12 +209,15 @@ 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", 100, &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", 20, &ControllerTabs::poseDataOnboardReceivedCallback, this);
+ // THE CONTROLLER THAT IS CURRENTLY OPERATING
controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
@@ -361,9 +364,9 @@ 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)
@@ -407,6 +410,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(pose_in_local_frame.vehicleName == m_object_name_for_emitting_pose_data)
+ //{
+ 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 +672,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", 20, &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 +684,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/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index a58cbc78..4974bcb7 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -265,9 +265,9 @@ class CrazyRadioClient:
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
+ #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
@@ -354,9 +354,9 @@ class CrazyRadioClient:
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");
+ #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");
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
index 21b594e2..50bcbf21 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
@@ -161,16 +161,25 @@ float yaml_battery_polling_period_in_seconds = 0.2f;
// // > Note that the delay in seconds therefore depends on the polling period
// int yaml_battery_delay_threshold_to_change_state = 5;
-// Frequency of requesting the onboard state estimate, in [seconds]
-float yaml_cfStateEstimate_polling_period_in_seconds = 0.2f;
+// VARIABLES FOR SIMULATING A QUAROTOR
// A QUADROTORS SIMULATOR
QuadrotorSimulator m_quadrotor_sim;
+// FREQUENCY OF REQUESTION ON BOARD STATE ESTIMATE, IN [seconds]
+float yaml_cfStateEstimate_polling_period_in_seconds = 0.02f;
+
+// 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;
+
diff --git a/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
index ff103cac..73270b35 100755
--- a/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
@@ -1,2 +1,15 @@
# Frequency of requesting the onboard state estimate, in [milliseconds]
-cfStateEstimate_polling_period: 20
\ No newline at end of file
+cfStateEstimate_polling_period: 20
+
+
+
+# ----------------------------------------------- #
+# THE FOLLOWING ARE USED FOR EMULATING A CRAZYFLIE:
+
+# QUADRATIC MOTOR REGRESSION EQUATION (a0, a1, a2)
+# > [Newtons] = a2 [cmd]^2 + a1 [cmd] + a0
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# THE MIN AND MAX FOR SATURATING THE 16-BIT THRUST COMMANDS
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 65000
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
index 6349dedc..b36f1464 100644
--- a/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/classes/QuadrotorSimulator.cpp
@@ -250,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;
@@ -259,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)"
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
index 9bad1acf..2d955693 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
@@ -296,9 +296,40 @@ void timerCallback_update_battery_voltage(const ros::TimerEvent&)
void timerCallback_update_cfStateEstimate(const ros::TimerEvent&)
{
- // Declare static paramters for the voltage
- static float voltage_current = 4.2;
-
+ // Simulate the quadrotor for one time step
+ m_quadrotor_sim.simulate_for_one_time_step( yaml_cfStateEstimate_polling_period_in_seconds );
+
+ // 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
+ cfStateEstimatorPublisher.publish(quadrotor_data);
}
@@ -419,8 +450,17 @@ void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle)
ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CrazyRadioConfig");
// Frequency of requesting the battery voltage, in [milliseconds]
- yaml_cfStateEstimate_polling_period_in_seconds = getParameterFloat(nodeHandle_for_paramaters,"cfStateEstimate_polling_period");
- yaml_cfStateEstimate_polling_period_in_seconds = yaml_cfStateEstimate_polling_period_in_seconds * 0.001;
+ float cfStateEstimate_polling_period_in_milliseconds = getParameterFloat(nodeHandle_for_paramaters,"cfStateEstimate_polling_period");
+ yaml_cfStateEstimate_polling_period_in_seconds = cfStateEstimate_polling_period_in_milliseconds * 0.001;
+
+ // 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/cfStateEstimate_polling_period = " << yaml_cfStateEstimate_polling_period_in_seconds);
@@ -530,7 +570,7 @@ int main(int argc, char* argv[])
fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
// Call the class function that loads the parameters for this class.
- fetchCrazyRadioConfigYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ fetchCrazyRadioConfigYamlParameters(nodeHandle);
@@ -549,15 +589,27 @@ int main(int argc, char* argv[])
// INITIALISE THE QUADROTOR SIMULATOR
- // > First conver the agentID to a zero-padded string
+ // > 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(3) << std::setfill('0') << m_agentID;
+ str_stream << std::setw(2) << std::setfill('0') << m_agentID;
std::string agentID_as_string(str_stream.str());
// > Initialise the quadrotor simulator class variable
- QuadrotorSimulator m_quadrotor_sim = QuadrotorSimulator( "CFSIM" + agentID_as_string , 0.032 );
+ 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();
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index c36e7bc4..638b98de 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -799,73 +799,98 @@ 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 );
+ }
}
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index ba64d084..ec9872d5 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -1495,22 +1495,22 @@ 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");
- // Flags for which measurement streams to use
- yaml_shouldUse_mocapMeasurements = getParameterBool(nodeHandle_for_paramaters,"shouldUse_mocapMeasurements");
+ // 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");
- // 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");
+ // 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
+ // 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
@@ -1523,24 +1523,24 @@ 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;
+ // PROCESS THE PARAMTERS
+ // Convert from degrees to radians
+ yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
- // Adjust the measurement subscribers
+ // 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");
// > keeps 50 messages because otherwise the next message may
- // overwrite the data while the current message is been used
+ // overwrite the data while the current message is been used
viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 50, viconCallback);
}
else
@@ -1564,8 +1564,8 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
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);
+ // 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);
}
--
GitLab