diff --git a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
index 2a9ae6dcd1ab0743fe1712cdd0a80553281e064f..ee4569312a33fc00851554fc595a34dc8cbf0ecf 100755
--- a/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
+++ b/dfall_ws/src/dfall_pkg/crazyradio/CrazyRadio.py
@@ -55,8 +55,10 @@ import struct
import logging
# Import the ROS bag package for logging
-import rosbag
-from rospkg import RosPack
+#import rosbag
+#from rospkg import RosPack
+
+# Import the ROS standard message types
from std_msgs.msg import Float32
from std_msgs.msg import String
@@ -133,11 +135,11 @@ FLYING_VEHICLE_STATE_TYPE_CRAZYFLIE_STATE_ESTIMATE = 2
# 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')
+#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:
"""
@@ -769,8 +771,8 @@ if __name__ == '__main__':
# Close the ROS loggin bag
- bag.close()
- rospy.loginfo("[CRAZY RADIO] Logging bag closed")
+ #bag.close()
+ #rospy.loginfo("[CRAZY RADIO] Logging bag closed")
# Close the CrazyRadio link
cf_client._cf.close_link()
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index de922337cfda598797d36915256aa311662bb180..5e09499b58d66bf629711c54b55a296f47f249b9 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.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
@@ -280,12 +296,27 @@ int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::s
// > For converting the global frame motion capture
// data to the local frame of this agent
void coordinatesToLocal(FlyingVehicleState& cf);
-// > Callback run when motion capture data has not
-// been receive for a specified time
-void timerCallback_mocap_timeout_check(const ros::TimerEvent&);
+
+
+
+// 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&);
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/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index 9e249f432eb2cea2e2c6c29ada9ef3dc3ee0e64a..ba64d0847db933703ec9d5f1b766c1d2d567287e 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
FlyingVehicleState poseDataForThisAgent;
+ poseDataForThisAgent.isValid = false;
// Extract the pose data for the allocated object from the
// full motion capture array
@@ -94,6 +90,8 @@ 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
FlyingVehicleState poseDataForOtherAgent;
poseDataForOtherAgent.isValid = false;
@@ -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's pose data is valid
- if(poseDataForThisAgent.isValid)
+ 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.isValid)"
+ } // END OF: "if(dataForThisAgent.isValid)"
}
else
@@ -293,104 +449,15 @@ 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 , 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 (
- (0 <= expected_index)
- and
- (expected_index < length_poseData)
- )
- {
- // Check if the names match
- if (viconData.crazyflies[expected_index].vehicleName == 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].vehicleName == 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;
- }
-}
-
-
-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;
}
-void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
-{
- // Update the flag
- m_isAvailable_mocap_data = false;
- // Inform the user
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Motion Capture Data has been unavailable for " << yaml_mocap_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 );
- }
-}
-
+// FUNCTION FOR SENDING A COMMAND, VIA THE CRAZYRADIO, THAT
+// THE MOTORS SHOULD BE SET TO ZERO
void sendZeroOutputCommandForMotors()
{
// Initialise a "control command" struct
@@ -407,8 +474,28 @@ void sendZeroOutputCommandForMotors()
zeroOutput.motorCmd2 = 0.0;
zeroOutput.motorCmd3 = 0.0;
zeroOutput.motorCmd4 = 0.0;
- // Publish the command
- m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
+ // Publish the command
+ m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
+}
+
+
+
+// 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_measurement_data = false;
+ // Inform the user
+ 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 );
+ }
}
@@ -432,60 +519,60 @@ void sendZeroOutputCommandForMotors()
// Checks if crazyflie is within allowed area
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;
}
@@ -573,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
@@ -627,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.");
}
}
}
@@ -1414,16 +1501,20 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
// 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");
+ 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");
// Number of consecutive occulsions that will deem the
// object as "long-term" occuled
- yaml_consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");
+ 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
@@ -1442,6 +1533,37 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
// 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");
+ // > keeps 50 messages because otherwise the next message may
+ // overwrite the data while the current message is been used
+ viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 50, 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);
+ // > keeps 20 messages because otherwise the next message may
+ // overwrite the data while the current message is been used
+ onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 20, 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);
}
@@ -1577,9 +1699,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();
@@ -1616,6 +1738,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
@@ -1634,9 +1760,21 @@ 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);
+ // > keeps 50 messages because otherwise the next message may
+ // overwrite the data while the current message is been used
+ if (yaml_shouldUse_mocapMeasurements)
+ {
+ viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 50, viconCallback);
+ }
+
+ // LOCALISATION DATA FROM ONBOARD THE FLYING VEHICLE
+ // > keeps 20 messages because otherwise the next message may
+ // overwrite the data while the current message is been used
+ if (yaml_shouldUse_onboardEstimate)
+ {
+ onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 20, onboardEstimateCallback);
+ }
// PUBLISHER FOR COMMANDING THE CRAZYFLIE
@@ -1674,9 +1812,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);