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 10fa29b6ea17cf095c7c88fc56da3e037ed36790..c1fca375177f67e0e947a0a10de7e17c48a08855 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
@@ -417,15 +417,15 @@ void ControllerTabs::poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehi
{
//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
- );
+ // 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
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
index 50bcbf213d7b339bc8254a75b92ed143b48c55cc..71481271b13d60a1b5d79b65993fb2869e2e9229 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
@@ -140,6 +140,10 @@ ros::Publisher cfStateEstimatorPublisher;
// the control command
ros::Publisher controlCommandPublisher;
+// Publisher for the simulation state
+// > This is used by the MocapEmulator node
+ros::Publisher cfSimulationStatePublisher;
+
@@ -170,8 +174,17 @@ float yaml_battery_polling_period_in_seconds = 0.2f;
// 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;
+// 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};
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 92077899385f643e3bd33a0f7def69cd83513a54..bb7a47f7d1f0f5bb61fb1d492fcef401ead9901e 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.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/MocapEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
index bfa09a7395cf0257ddac5f35d8fcf3cc00ec583b..d73349f7da1e55dba4009506e239c6f103a57122 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/MocapEmulator.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/param/CrazyRadioConfig.yaml b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
index 73270b35ec1bce084b168963f90ba62792d09241..88ddd0a794bc2963d7d810abe6ebf13c71b47a19 100755
--- a/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/CrazyRadioConfig.yaml
@@ -6,6 +6,16 @@ cfStateEstimate_polling_period: 20
# ----------------------------------------------- #
# 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]
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/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/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
index 2d9556930e577a2e9882b06f1b34649621e5ee2f..87af0d62cd1b02d881af12c44219bf6afb663861 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
@@ -296,8 +296,14 @@ void timerCallback_update_battery_voltage(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_cfStateEstimate_polling_period_in_seconds );
+ m_quadrotor_sim.simulate_for_one_time_step( yaml_cfSimulation_deltaT_in_seconds );
// Local variable for the data of this quadrotor
FlyingVehicleState quadrotor_data;
@@ -328,8 +334,20 @@ void timerCallback_update_cfStateEstimate(const ros::TimerEvent&)
// > For the acquiring time
quadrotor_data.acquiringTime = 0.0;
- // Publish the current state
- cfStateEstimatorPublisher.publish(quadrotor_data);
+ // 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;
+ }
}
@@ -446,12 +464,14 @@ void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle)
// Here we load the parameters that are specified in the file:
// BatteryMonitor.yaml
- // Add the "BatteryMonitor" namespace to the "nodeHandle"
+ // Add the "CrazyRadioConfig" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CrazyRadioConfig");
- // Frequency of requesting the battery voltage, in [milliseconds]
- 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;
+ // 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);
@@ -463,7 +483,16 @@ void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle)
// 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);
+ //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);
}
@@ -581,7 +610,7 @@ int main(int argc, char* argv[])
// INITIALISE TIMER FOR THE STATE ESTIMATE PUBLISHER TIMER
- m_timer_state_estimate_update = nodeHandle.createTimer(ros::Duration(yaml_cfStateEstimate_polling_period_in_seconds), timerCallback_update_cfStateEstimate, false);
+ 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();
@@ -638,6 +667,10 @@ int main(int argc, char* argv[])
// 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/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index 638b98de6a140f6b502d3d1dd55f451e05b77487..a412a8849b18df85af9723d256a8d7d4f1672d0d 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -870,7 +870,6 @@ void performEstimatorUpdate_forStateInterial(Controller::Request &request)
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
@@ -912,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
{
@@ -1410,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;
}
@@ -1600,28 +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);
+ CMQuery contextCall;
+ contextCall.request.studentID = m_agentID;
+ ROS_INFO_STREAM("[DEFAULT CONTROLLER] AgentID:" << m_agentID);
+
+ CrazyflieContext new_context;
- CrazyflieContext new_context;
+ m_centralManager.waitForExistence(ros::Duration(-1));
- m_centralManager.waitForExistence(ros::Duration(-1));
+ if(m_centralManager.call(contextCall)) {
+ new_context = contextCall.response.crazyflieContext;
- if(m_centralManager.call(contextCall)) {
- new_context = contextCall.response.crazyflieContext;
+ 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");
+ }
}
@@ -1847,6 +1849,8 @@ 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
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
index ceca93a87b234f8a260516a74d88c9741662148f..54fa2eee667c074f288f84d2992baf5f2525403f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/MocapEmulator.cpp
@@ -58,29 +58,32 @@
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
FlyingVehicleState quadrotor_data;
@@ -89,17 +92,17 @@ void timerCallback_mocap_publisher(const ros::TimerEvent&)
// > For the type
quadrotor_data.type = FLYING_VEHICLE_STATE_TYPE_MOCAP_MEASUREMENT;
// > For the name
- quadrotor_data.vehicleName = it->get_id_string();
+ quadrotor_data.vehicleName = it->vehicleName;
// > For the occulsion flag
- quadrotor_data.isValid = true;
+ 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;
@@ -116,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
@@ -147,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->get_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;
@@ -178,12 +196,12 @@ 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->get_id_string() << "\" connected to agent ID " << new_agent_id << " in database.");
@@ -192,14 +210,14 @@ void loadContextForEachQuadrotor()
{
// 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->get_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 );
}
}
@@ -222,11 +240,11 @@ 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");
@@ -239,18 +257,18 @@ void fetchMocapEmulatorConfigYamlParameters()
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);
}
@@ -293,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();
}
@@ -348,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/StudentControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
index 292113d3f5e02a4b56b2db908d2fd06d9e2dbe06..424a48dfcc9b4c6c7436bd346ed48663a2f4dcec 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/StudentControllerService.cpp
@@ -222,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