diff --git a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
index 8da4fa3c57fbb27eee1b23ef86866179343458f6..12e9add738047345367f8d7027766914fdb1c553 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/DefaultControllerService.h
@@ -67,6 +67,7 @@
#include "dfall_pkg/DebugMsg.h"
// Include the DFALL service types
+#include "dfall_pkg/IntIntService.h"
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
index 863c8c7c530336224560bf33e88b662bcdbe8d35..f315d7de5fc251e0efd66439652b5683b72b60cf 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/FlyingAgentClient.h
@@ -138,6 +138,8 @@ int yaml_consecutive_occulsions_threshold = 10;
// > 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;
@@ -148,7 +150,7 @@ ros::Timer m_timer_mocap_timeout_check;
bool yaml_isEnabled_strictSafety = true;
// > The maximum allowed tilt angle, in degrees and radians
float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
-float m_maxTiltAngle_for_strictSafety_redians = 70 * DEG2RAD;
+float m_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
@@ -158,7 +160,7 @@ int m_flying_state;
// > Booleans for whether the {take-off,landing} should
// be performed with the default controller
bool yaml_shouldPerfom_takeOff_with_defaultController = true;
-bool yaml_shouldPerfom_takeOff_with_defaultController = true;
+bool yaml_shouldPerfom_landing_with_defaultController = true;
// > Service Clients for requesting the Default controller
// to perform a {take-off,landing} maneouvre
ros::ServiceClient m_defaultController_requestManoeuvre;
@@ -180,7 +182,7 @@ ros::ServiceClient* m_instant_controller_service_client;
bool m_controllers_avialble = false;
// > Timer for creating the controller service client after
// some delay
-ros::Timer timer_for_loadAllControllers;
+ros::Timer m_timer_for_createAllControllerServiceClients;
// > Integer indicating the controller that has been
// requested. This controller is used during the "flying"
// state, and the "Default" controller is used during the
@@ -220,10 +222,10 @@ ros::ServiceClient centralManager;
// to the Crazyflie (the CrazyRadio node listen to this
// publisher and actually send the commands)
// {onboardControllerType,roll,pitch,yaw,motorCmd1,motorCmd2,motorCmd3,motorCmd4}
-ros::Publisher commandForSendingToCrazyfliePublisher;
+ros::Publisher m_commandForSendingToCrazyfliePublisher;
// Publisher for the current flying state of this Flying Agent Client
-ros::Publisher flyingStatePublisher;
+ros::Publisher m_flyingStatePublisher;
// Publisher for the commands of:
// {take-off,land,motors-off, and which controller to use}
@@ -288,10 +290,9 @@ void requestChangeFlyingStateToTakeOff();
// > For changing to land
void requestChangeFlyingStateToLand();
// > Callback that the take off phase is complete
-void takeOffTimerCallback(const ros::TimerEvent&)
+void takeOffTimerCallback(const ros::TimerEvent&);
// > Callback that the landing phase is complete
-void landTimerCallback(const ros::TimerEvent&)
-
+void landTimerCallback(const ros::TimerEvent&);
@@ -347,6 +348,9 @@ void loadCrazyflieContext();
// > For creating a service client from the name
// of the YAML parameter
void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
+// > For creating an IntInt service client from the
+// name of a YAML paramter
+void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
// > Callback for the timer so that all services servers
// exists before we try to create the clients
void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&);
diff --git a/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml b/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
index a5825598368247f9d1fd917af07971365b5aa3c6..b6d75db6044bcd6e65422b4a3779e17a71fae6b3 100755
--- a/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
+++ b/dfall_ws/src/dfall_pkg/param/ClientConfig.yaml
@@ -19,6 +19,9 @@ maxTiltAngle_for_strictSafety_degrees: 70
# object as "long-term" occuled
consecutive_occulsions_threshold: 10
+# Time out duration after which Mocap is considered unavailable
+mocap_timeout_duration: 2.0
+
# Flag for whether the take-off should be performed
# with the default controller
shouldPerfom_takeOff_with_defaultController: true
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
index d6e0a5d8e1b8d336029cdad4892b946fee6454a3..be6ec8343035316885513735508c3ffb611e9b89 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/DefaultControllerService.cpp
@@ -214,7 +214,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (m_current_state == DEFAULT_CONTROLLER_STATE_TAKE_OFF_SPIN_MOTORS)
{
// Compute the "spinning" thrust
- float thrust_for_spinning = 1000.0 + min(0.4,m_time_in_seconds) * 10000.0;
+ float thrust_for_spinning = 1000.0;
+ if (m_time_in_seconds < 0.8)
+ thrust_for_spinning += m_time_in_seconds * 10000.0;
+ else
+ thrust_for_spinning += 8000.0;
response.controlOutput.roll = 0.0;
response.controlOutput.pitch = 0.0;
@@ -552,7 +556,7 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
response.setpointWithHeader.z = m_setpoint[2];
response.setpointWithHeader.yaw = m_setpoint[3];
// Put the current state into the "buttonID" field
- response.buttonID = m_current_state;
+ response.setpointWithHeader.buttonID = m_current_state;
// Return
return true;
}
@@ -569,7 +573,7 @@ void publishCurrentSetpointAndState()
msg.z = m_setpoint[2];
msg.yaw = m_setpoint[3];
// Put the current state into the "buttonID" field
- response.buttonID = m_current_state;
+ msg.buttonID = m_current_state;
// Publish the message
m_setpointChangedPublisher.publish(msg);
}
@@ -990,14 +994,6 @@ int main(int argc, char* argv[])
- // Instantiate the class variable "m_stateChangedPublisher" to
- // be a "ros::Publisher". This variable advertises under the name
- // "SetpointChanged" and is a message with the structure defined
- // in the file "IntWithHeader.msg" (located in the "msg" folder).
- // This publisher is used by the "flying agent GUI" to update the
- // field that displays the current state for this controller.
- m_stateChangedPublisher = nodeHandle.advertise<IntWithHeader>("StateChanged", 1);
-
// Print out some information to the user.
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
index e48d3d11dd53c6ce5935c840fbff1f40759c88a2..288af49ad4b62593da4ef551a145c34f30186a53 100755
--- a/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/FlyingAgentClient.cpp
@@ -106,81 +106,97 @@ void viconCallback(const ViconData& viconData)
// 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
if (
(m_poseDataIndex >= 0)
and
- (m_flying_state != STATE_MOTORS_OFF)
- and
(m_controllers_avialble)
)
{
- // Initliase the "Contrller" service call variable
- Controller controllerCall;
-
- // Fill in the pose data for this agent
- controllerCall.request.ownCrazyflie = poseDataForThisAgent;
- // If the object is NOT occluded,
- // then proceed to make the "Controller Service Call" that
- // compute the controller output.
+ // Only continue if:
+ // (1) the agent is NOT occulded
if(!poseDataForThisAgent.occluded)
{
+ // Update the flag
+ m_isOcculded_mocap_data = false;
// Reset the "consecutive occulsions" flag
number_of_consecutive_occulsions = 0;
- // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
- if ( getInstantController() != DEFAULT_CONTROLLER )
+
+ // Only continue if:
+ // (1) The state is different from MOTORS-OFF
+ if (m_flying_state != STATE_MOTORS_OFF)
{
- if ( !safetyCheck_on_positionAndTilt(poseDataForThisAgent) )
- {
- setInstantController(DEFAULT_CONTROLLER);
- ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
- }
- }
- // Initialise a local boolean success variable
- bool isSuccessful_controllerCall = false;
+ // Initliase the "Contrller" service call variable
+ Controller controllerCall;
- // Call the controller service client
- isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+ // Fill in the pose data for this agent
+ controllerCall.request.ownCrazyflie = poseDataForThisAgent;
- // Ensure success and enforce safety
- if(!isSuccessful_controllerCall)
- {
- // Let the user know that the controller call failed
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+
+
- // Switch to the default controller,
- // if it was not already the active controller
+ // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
if ( getInstantController() != DEFAULT_CONTROLLER )
{
- // Set the DEFAULT controller to be active
- setInstantController(DEFAULT_CONTROLLER);
- // Try the controller call
- isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
- // Inform the user is not successful
- if ( !isSuccessful_controllerCall )
+ if ( !safetyCheck_on_positionAndTilt(poseDataForThisAgent) )
{
- ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Also failed to call the DEFAULT controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+ setInstantController(DEFAULT_CONTROLLER);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
}
}
- }
- // Send the command to the CrazyRadio
- // > IF SUCCESSFUL
- if (isSuccessful_controllerCall)
- {
- m_commaNdfOrsendiNgTocrazyflIepublisher.publish(controllerCall.response.controlOutput);
+ // Initialise a local boolean success variable
+ bool isSuccessful_controllerCall = false;
+
+ // Call the controller service client
+ isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+
+ // Ensure success and enforce safety
+ if(!isSuccessful_controllerCall)
+ {
+ // Let the user know that the controller call failed
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+
+ // Switch to the default controller,
+ // if it was not already the active controller
+ if ( getInstantController() != DEFAULT_CONTROLLER )
+ {
+ // Set the DEFAULT controller to be active
+ setInstantController(DEFAULT_CONTROLLER);
+ // Try the controller call
+ isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
+ // Inform the user is not successful
+ if ( !isSuccessful_controllerCall )
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Also failed to call the DEFAULT controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
+ }
+ }
+ }
+
+ // Send the command to the CrazyRadio
+ // > IF SUCCESSFUL
+ if (isSuccessful_controllerCall)
+ {
+ m_commandForSendingToCrazyfliePublisher.publish(controllerCall.response.controlOutput);
+ }
+ // > ELSE SEND ZERO OUTPUT COMMAND
+ else
+ {
+ // Send the command to turn the motors off
+ sendZeroOutputCommandForMotors();
+ // And change the state to motor-off
+ requestChangeFlyingStateTo(STATE_MOTORS_OFF);
+ }
}
- // > ELSE SEND ZERO OUTPUT COMMAND
else
{
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
- // And change the state to motor-off
- requestChangeFlyingStateTo(STATE_MOTORS_OFF);
- }
+ } // END OF: "if (m_flying_state != STATE_MOTORS_OFF)"
}
else
{
@@ -195,6 +211,8 @@ void viconCallback(const ViconData& viconData)
{
// Update the flag
m_isOcculded_mocap_data = true;
+ // Inform the user
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Agent was occluded for more than " << yaml_consecutive_occulsions_threshold << " consecutive frames." );
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
// Update the flying state
@@ -208,7 +226,7 @@ void viconCallback(const ViconData& viconData)
// Send the command to turn the motors off
sendZeroOutputCommandForMotors();
- } // END OF: "if ( (m_poseDataIndex >= 0) and (m_flying_state != STATE_MOTORS_OFF) )"
+ } // END OF: "if ( (m_poseDataIndex >= 0) and (m_controllers_avialble) )"
}
@@ -306,7 +324,7 @@ void sendZeroOutputCommandForMotors()
{
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
- m_commaNdfOrsendiNgTocrazyflIepublisher.publish(zeroOutput);
+ m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
}
@@ -359,9 +377,9 @@ bool safetyCheck_on_positionAndTilt(CrazyflieData data)
{
// Check on the ROLL angle
if(
- (data.roll > m_maxTiltAngle_for_strictSafety_redians)
+ (data.roll > m_maxTiltAngle_for_strictSafety_radians)
or
- (data.roll < -m_maxTiltAngle_for_strictSafety_redians)
+ (data.roll < -m_maxTiltAngle_for_strictSafety_radians)
)
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
@@ -369,9 +387,9 @@ bool safetyCheck_on_positionAndTilt(CrazyflieData data)
}
// Check on the PITCH angle
if(
- (data.pitch > m_maxTiltAngle_for_strictSafety_redians)
+ (data.pitch > m_maxTiltAngle_for_strictSafety_radians)
or
- (data.pitch < -m_maxTiltAngle_for_strictSafety_redians)
+ (data.pitch < -m_maxTiltAngle_for_strictSafety_radians)
)
{
ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
@@ -456,7 +474,7 @@ void requestChangeFlyingStateTo(int new_state)
// Publish a message with the new flying state
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = m_flying_state;
- flyingStatePublisher.publish(flying_state_msg);
+ m_flyingStatePublisher.publish(flying_state_msg);
}
void requestChangeFlyingStateToTakeOff()
@@ -568,7 +586,7 @@ void requestChangeFlyingStateToLand()
// > Stop any previous instance
m_timer_land_complete.stop();
// > Set the period again (second argument is reset)
- m_timer_land_complete.setPeriod( ros::Duration(take_off_duration), true);
+ m_timer_land_complete.setPeriod( ros::Duration(land_duration), true);
// > Start the timer
m_timer_land_complete.start();
// Inform the user
@@ -606,6 +624,10 @@ void timerCallback_takeoff_complete(const ros::TimerEvent&)
ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
// Update the class variable
m_flying_state = STATE_FLYING;
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
}
else
{
@@ -623,6 +645,10 @@ void timerCallback_land_complete(const ros::TimerEvent&)
ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
// Update the class variable
m_flying_state = STATE_MOTORS_OFF;
+ // Publish a message with the new flying state
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = m_flying_state;
+ m_flyingStatePublisher.publish(flying_state_msg);
}
else
{
@@ -1067,6 +1093,23 @@ void createControllerServiceClientFromParameterName( std::string paramter_name ,
}
+void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
+{
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
+
+ std::string controllerName;
+ if(!nodeHandle.getParam(paramter_name, controllerName))
+ {
+ ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
+ return;
+ }
+
+ serviceClient = ros::service::createClient<IntIntService>(controllerName, true);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() << ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
+}
+
+
void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
{
@@ -1079,11 +1122,11 @@ void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
// INITIALISE THE SERVICE FOR REQUESTING THE DEFAULT
// CONTROLLER TO PERFORM MANOEUVRES
- createControllerServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
+ createIntIntServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
// Check that at least the default controller is available
// > Setting the flag accordingly
- if (defaultController)
+ if (m_defaultController)
{
m_controllers_avialble = true;
}
@@ -1175,7 +1218,10 @@ void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
// Number of consecutive occulsions that will deem the
// object as "long-term" occuled
- consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");
+ yaml_consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");
+
+ // Time out duration after which Mocap is considered unavailable
+ yaml_mocap_timeout_duration = getParameterFloat(nodeHandle_for_paramaters,"mocap_timeout_duration");
// Flag for whether the take-off should be performed
// with the default controller
@@ -1192,10 +1238,10 @@ void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
// PROCESS THE PARAMTERS
// Convert from degrees to radians
- m_maxTiltAngle_for_strictSafety_redians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
+ m_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
// DEBUGGING: Print out one of the processed values
- ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing m_maxTiltAngle_for_strictSafety_redians = " << m_maxTiltAngle_for_strictSafety_redians);
+ ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing m_maxTiltAngle_for_strictSafety_radians = " << m_maxTiltAngle_for_strictSafety_radians);
}
@@ -1312,6 +1358,13 @@ int main(int argc, char* argv[])
m_timer_mocap_timeout_check.stop();
+ // INITIALISE THE TAKE-OFF AND LANDING COMPLETE TIMERS
+ // > And stop it immediately
+ m_timer_takeoff_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
+ m_timer_takeoff_complete.stop();
+ m_timer_land_complete = nodeHandle.createTimer(ros::Duration(1.0), timerCallback_takeoff_complete, true);
+ m_timer_land_complete.stop();
+
@@ -1353,7 +1406,7 @@ int main(int argc, char* argv[])
// PUBLISHER FOR COMMANDING THE CRAZYFLIE
// i.e., motorCmd{1,2,3,4}, roll, pitch, yaw, and onboardControllerType
//ros::Publishers to advertise the control output
- m_commaNdfOrsendiNgTocrazyflIepublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
+ m_commandForSendingToCrazyfliePublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
//this topic lets the FlyingAgentClient listen to the terminal commands
//commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
@@ -1381,7 +1434,7 @@ int main(int argc, char* argv[])
// PUBLISHER FOR THE FLYING STATE
// Possible states: {MOTORS-OFF,TAKE-OFF,FLYING,LAND}
// This topic will publish flying state whenever it changes.
- flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
+ m_flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
@@ -1397,7 +1450,7 @@ int main(int argc, char* argv[])
// publish first flying state data
std_msgs::Int32 flying_state_msg;
flying_state_msg.data = m_flying_state;
- flyingStatePublisher.publish(flying_state_msg);
+ m_flyingStatePublisher.publish(flying_state_msg);
// SafeControllerServicePublisher:
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();