To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 0306c4c9 authored by beuchatp's avatar beuchatp
Browse files

Added message so that the default controller informs the flying agent client...

Added message so that the default controller informs the flying agent client when a manoeuvre is complete
parent 43fa29a6
......@@ -39,6 +39,14 @@
#define DEFAULT_CONTROLLER_REQUEST_LANDING 2
// TO NOTIFY THAT MANOEUVRES ARE COMPLETED
#define DEFAULT_CONTROLLER_TAKEOFF_COMPLETE 1
#define DEFAULT_CONTROLLER_LANDING_COMPLETE 2
// TO IDENITFY THE STATE OF THE DEFAULT CONTROLLER
#define DEFAULT_CONTROLLER_STATE_NORMAL 1
......
......@@ -369,6 +369,9 @@ ros::Publisher m_debugPublisher;
// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
// ROS Publisher to inform the flying agent client
// when a requested manoeuvre is complete
ros::Publisher m_manoeuvreCompletePublisher;
// ROS Publisher for sending motors-off command
// to the flying agent client
......
......@@ -296,6 +296,8 @@ void requestChangeFlyingStateToLand();
void takeOffTimerCallback(const ros::TimerEvent&);
// > Callback that the landing phase is complete
void landTimerCallback(const ros::TimerEvent&);
// > Callback that the Default Controller complete a manoeuvre
void defaultControllerManoeuvreCompleteCallback(const IntWithHeader & msg);
......
......@@ -42,7 +42,7 @@ takoff_goto_setpoint_max_time: 4.0
takoff_integrator_on_box_horizontal: 0.25
takoff_integrator_on_box_vertical: 0.15
# The time for: take off integrator-on
takoff_integrator_on_time: 3.0
takoff_integrator_on_time: 4.0
# Height change for the landing move-down
......@@ -99,16 +99,16 @@ gainMatrixThrust_2StateVector : [ 0.98, 0.25]
gainMatrixRollRate_3StateVector : [-6.20,-3.00, 5.20]
gainMatrixPitchRate_3StateVector : [ 6.20, 3.00, 5.20]
# The LQR Controller parameters for mode 2 (Angle-nested)
gainMatrixRollAngle_2StateVector : [-0.20,-0.20]
gainMatrixPitchAngle_2StateVector : [ 0.20, 0.20]
gainMatrixRollAngle_2StateVector : [-0.50,-0.30]
gainMatrixPitchAngle_2StateVector : [ 0.50, 0.30]
gainRollRate_fromAngle : 4.00
gainPitchRate_fromAngle : 4.00
# The LQR Controller parameters for yaw
gainYawRate_fromAngle : 2.30
# Integrator gains
integratorGain_forThrust : 0.10
integratorGain_forTauXY : 0.00
integratorGain_forTauYaw : 0.00
integratorGain_forTauXY : 0.06
integratorGain_forTauYaw : 0.05
......
......@@ -538,12 +538,16 @@ void computeResponse_for_takeoff_integrator_on(Controller::Response &response)
if (m_time_in_seconds > yaml_takoff_integrator_on_time)
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER] Switch to state: normal");
ROS_INFO("[DEFAULT CONTROLLER] Publish message that take-off is complete, and switch to state: normal");
// Reset the time variable
m_time_in_seconds = 0.0;
// Update the state accordingly
m_current_state = DEFAULT_CONTROLLER_STATE_NORMAL;
m_current_state_changed = true;
// Publish a message that the take-off is complete
IntWithHeader msg;
msg.data = DEFAULT_CONTROLLER_TAKEOFF_COMPLETE;
m_manoeuvreCompletePublisher.publish(msg);
}
}
......@@ -664,12 +668,16 @@ void computeResponse_for_landing_spin_motors(Controller::Response &response)
if ( m_time_in_seconds > (0.7*yaml_landing_spin_motors_time) )
{
// Inform the user
ROS_INFO("[DEFAULT CONTROLLER] Switch to state: standby");
ROS_INFO("[DEFAULT CONTROLLER] Publish message that landing is complete, and switch to state: standby");
// Reset the time variable
m_time_in_seconds = 0.0;
// Update the state accordingly
m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;
m_current_state_changed = true;
// Publish a message that the take-off is complete
IntWithHeader msg;
msg.data = DEFAULT_CONTROLLER_LANDING_COMPLETE;
m_manoeuvreCompletePublisher.publish(msg);
}
}
}
......@@ -910,19 +918,19 @@ void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateI
stateInertialError[6] = stateInertial[6];
stateInertialError[7] = stateInertial[7];
// Clip the x-coordination to within the specified bounds
// Clip the x-error to within the specified bounds
if (stateInertialError[0] > yaml_max_setpoint_error_xy)
stateInertialError[0] = yaml_max_setpoint_error_xy;
else if (stateInertialError[0] < -yaml_max_setpoint_error_xy)
stateInertialError[0] = -yaml_max_setpoint_error_xy;
// Clip the y-coordination to within the specified bounds
// Clip the y-error to within the specified bounds
if (stateInertialError[1] > yaml_max_setpoint_error_xy)
stateInertialError[1] = yaml_max_setpoint_error_xy;
else if (stateInertialError[1] < -yaml_max_setpoint_error_xy)
stateInertialError[1] = -yaml_max_setpoint_error_xy;
// Clip the z-coordination to within the specified bounds
// Clip the z-error to within the specified bounds
if (stateInertialError[2] > yaml_max_setpoint_error_z)
stateInertialError[2] = yaml_max_setpoint_error_z;
else if (stateInertialError[2] < -yaml_max_setpoint_error_z)
......@@ -1916,6 +1924,15 @@ int main(int argc, char* argv[])
// will take to perform (in milliseconds)
ros::ServiceServer requestManoeuvreService = nodeHandle.advertiseService("RequestManoeuvre", requestManoeuvreCallback);
// Instantiate the class variable "m_manoeuvreCompletePublisher" to
// be a "ros::Publisher". This variable advertises under the name
// "ManoeuvreComplete" 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
// flying state once the manoeuvre is complete.
m_manoeuvreCompletePublisher = nodeHandle.advertise<IntWithHeader>("ManoeuvreComplete", 1);
// Instantiate the class variable "m_motorsOffToFlyingAgentClientPublisher"
// to be a "ros::Publisher". This variable advertises under the
// name space:
......
......@@ -663,8 +663,8 @@ void requestChangeFlyingStateToLand()
void timerCallback_takeoff_complete(const ros::TimerEvent&)
{
// Only change to flying if still in the take-off state
if (m_flying_state == STATE_TAKE_OFF)
// Only change to flying if still in the take-off state
if (m_flying_state == STATE_TAKE_OFF)
{
// Inform the user
ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
......@@ -686,8 +686,8 @@ void timerCallback_takeoff_complete(const ros::TimerEvent&)
void timerCallback_land_complete(const ros::TimerEvent&)
{
// Only change to flying if still in the take-off state
if (m_flying_state == STATE_LAND)
// Only change to flying if still in the take-off state
if (m_flying_state == STATE_LAND)
{
// Inform the user
ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
......@@ -710,6 +710,67 @@ void timerCallback_land_complete(const ros::TimerEvent&)
void defaultControllerManoeuvreCompleteCallback(const IntWithHeader & msg)
{
// Switch between the cases
switch (msg.data)
{
case DEFAULT_CONTROLLER_TAKEOFF_COMPLETE:
{
// Only change to flying if still in the take-off state
if (m_flying_state == STATE_TAKE_OFF)
{
// Stop the timer
m_timer_takeoff_complete.stop();
// Inform the user
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);
// Change back to the nominal controller
setInstantController( m_controller_nominally_selected );
}
else
{
// Inform the user
ROS_INFO("[FLYING AGENT CLIENT] Received a take-off complete message, BUT the agent is no longer in STATE_TAKE_OFF.");
}
break;
}
case DEFAULT_CONTROLLER_LANDING_COMPLETE:
{
// Only change to flying if still in the take-off state
if (m_flying_state == STATE_LAND)
{
// Stop the timer
m_timer_land_complete.stop();
// Inform the user
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);
// Change back to the nominal controller
setInstantController( m_controller_nominally_selected );
}
else
{
// Inform the user
ROS_INFO("[FLYING AGENT CLIENT] Received a landing complete message, BUT the agent is no longer in STATE_LAND.");
}
break;
}
}
}
......@@ -1564,6 +1625,15 @@ int main(int argc, char* argv[])
ros::Subscriber CFBatterySubscriber = nodeHandle_to_battery_monitor.subscribe("ChangedStateTo", 1, batteryMonitorStateChangedCallback);
// SUBSCRIBER FOR BATTERY STATE CHANGES
// The battery state change message from the Battery
// Monitor node
std::string namespace_to_default_contoller = m_namespace + "/DefaultControllerService";
ros::NodeHandle nodeHandle_to_default_controller(namespace_to_default_contoller);
ros::Subscriber ManoeuvreCompleteSubscriber = nodeHandle_to_default_controller.subscribe("ManoeuvreComplete", 1, defaultControllerManoeuvreCompleteCallback);
// SERVICE SERVER FOR OTHERS TO GET THE CURRENT FLYING STATE
// Advertise the service that return the "m_flying_state"
// variable when called upon
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment