Commit e8972449 authored by beuchatp's avatar beuchatp
Browse files

Got most everything working for the web interface

parent cfa79e99
......@@ -252,6 +252,20 @@ class CrazyRadioClient:
def _data_received_stateEstimate_callback(self, timestamp, data, logconf):
# Perform safety check if required
if (isEnabled_strictSafety):
# Estimate the height at the next measurement
height_at_next_measurement = data["stateEstimateZ.z"] / 1000.0 + (cfStateEstimate_polling_period / 1000.0) * (data["stateEstimateZ.vz"] / 1000.0)
# Turn-off if too high
if (height_at_next_measurement > maxHeight_for_strictSafety_meters):
# Publish a motors OFF command
msg = IntWithHeader()
msg.shouldCheckForAgentID = False
msg.data = CMD_CRAZYFLY_MOTORS_OFF
self.flyingAgentClient_command_publisher.publish(msg)
# Inform the user
rospy.logerr("[CRAZY RADIO] Height safety check failed, measured = %f, max allowed = %f" % (height_at_next_measurement, maxHeight_for_strictSafety_meters))
# Initialise the variable for the flying vehicle state
cfStateEstimate = FlyingVehicleState()
......@@ -747,6 +761,12 @@ if __name__ == '__main__':
global cfStateEstimate_polling_period
cfStateEstimate_polling_period = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/cfStateEstimate_polling_period")
global isEnabled_strictSafety
isEnabled_strictSafety = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/isEnabled_strictSafety")
global maxHeight_for_strictSafety_meters
maxHeight_for_strictSafety_meters = rospy.get_param(ros_namespace + "CrazyRadio/CrazyRadioConfig/maxHeight_for_strictSafety_meters")
# Fetch the YAML paramter "agentID" and "coordID"
global m_agentID
m_agentID = rospy.get_param(ros_namespace + "/FlyingAgentClient/agentID")
......
......@@ -56,6 +56,8 @@
// Include the DFALL message types
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/FlyingVehicleState.h"
#include "dfall_pkg/DebugMsg.h"
// Include the DFALL service types
#include "dfall_pkg/IntStringService.h"
......@@ -95,6 +97,10 @@ using namespace dfall_pkg;
// > as received via messages
int m_crazyradio_status = CRAZY_RADIO_STATE_DISCONNECTED;
// The battery level
// > as received via messages
int m_battery_level = BATTERY_LEVEL_UNAVAILABLE;
// The flying state of the agent
// > as received via messages
int m_agent_operating_state = STATE_UNAVAILABLE;
......@@ -103,10 +109,6 @@ int m_agent_operating_state = STATE_UNAVAILABLE;
// > as received via messages
int m_instant_controller = DEFAULT_CONTROLLER;
// The battery level
// > as received via messages
int m_battery_level = BATTERY_LEVEL_UNAVAILABLE;
// The setpoint of the default controller
// > as received via messages
float m_setpoint_default[4] = {0.0,0.0,0.4,0.0};
......@@ -115,6 +117,14 @@ float m_setpoint_default[4] = {0.0,0.0,0.4,0.0};
// > as received via messages
float m_setpoint_student[4] = {0.0,0.0,0.4,0.0};
// The debug values of the student controller
// > as received via messages
float m_debug_values_student[10] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
// The current state estimate of the Crazyflie
// > as received via messages
float m_cf_state_estimate_xyz_rpy[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
......@@ -137,16 +147,20 @@ float m_setpoint_student[4] = {0.0,0.0,0.4,0.0};
// Callbacks for messages with the status of things:
// > For the status of the crazyradio
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
// > For the battery level
void newBatteryLevelCallback(const std_msgs::Int32& msg);
// > For the flying state of the agent
void agentOperatingStateCallback(const std_msgs::Int32& msg);
// > For the instant controller
void instantControllerChangedCallback(const std_msgs::Int32& msg);
// > For the battery level
void newBatteryLevelCallback(const std_msgs::Int32& msg);
// > For the Default Controller Setpoint
void defaultControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint);
// > For the Student Controller Setpoint
void studentControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint);
// > For the Student Controller Debug Values
void studentControllerDebugValuesCallback(const DebugMsg& newDebugMsg);
// > For the State Estimate from the Crazyflie
void cfStateEstimateCallback(const FlyingVehicleState & newStateEstimate);
// Service callback for providing status to the Web Interface
bool statusForWebInterfaceCallback(IntStringService::Request &request, IntStringService::Response &response);
\ No newline at end of file
......@@ -153,6 +153,11 @@ ros::Publisher cfSimulationStatePublisher;
// Frequency of requesting the battery voltage, in [seconds]
float yaml_battery_polling_period_in_seconds = 0.2f;
// Flag for whether to use height as a trigger for
// publishing a motors-OFF command
bool yaml_isEnabled_strictSafety = true;
float yaml_maxHeight_for_strictSafety_meters = 1.2;
// // Battery thresholds while in the "motors off" state, in [Volts]
// float yaml_battery_voltage_threshold_lower_while_standby = 3.30f;
// float yaml_battery_voltage_threshold_upper_while_standby = 4.20f;
......
......@@ -69,7 +69,7 @@
// Include the DFALL service types
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
#include "dfall_pkg/GetDebugValuesService.h"
//#include "dfall_pkg/GetDebugValuesService.h"
// Include the shared definitions
#include "nodes/Constants.h"
......
# Frequency of requesting the onboard state estimate, in [milliseconds]
cfStateEstimate_polling_period: 20
# Flag for whether to use height as a trigger for
# publishing a motors-OFF command
isEnabled_strictSafety: true
maxHeight_for_strictSafety_meters: 1.2
# ----------------------------------------------- #
......
......@@ -70,6 +70,11 @@ void crazyRadioStatusCallback(const std_msgs::Int32& msg)
m_crazyradio_status = msg.data;
}
// > For the battery level
void newBatteryLevelCallback(const std_msgs::Int32& msg)
{
m_battery_level = msg.data;
}
// > For the flying state of the agent
void agentOperatingStateCallback(const std_msgs::Int32& msg)
......@@ -83,12 +88,6 @@ void instantControllerChangedCallback(const std_msgs::Int32& msg)
m_instant_controller = msg.data;
}
// > For the battery level
void newBatteryLevelCallback(const std_msgs::Int32& msg)
{
m_battery_level = msg.data;
}
// > For the Default Controller Setpoint
void defaultControllerSetpointChangedCallback(const SetpointWithHeader& newSetpoint)
{
......@@ -107,6 +106,34 @@ void studentControllerSetpointChangedCallback(const SetpointWithHeader& newSetpo
m_setpoint_student[3] = newSetpoint.yaw;
}
// > For the Student Controller Debug Values
void studentControllerDebugValuesCallback(const DebugMsg& newDebugMsg)
{
m_debug_values_student[0] = newDebugMsg.value_1;
m_debug_values_student[1] = newDebugMsg.value_2;
m_debug_values_student[2] = newDebugMsg.value_3;
m_debug_values_student[3] = newDebugMsg.value_4;
m_debug_values_student[4] = newDebugMsg.value_5;
m_debug_values_student[5] = newDebugMsg.value_6;
m_debug_values_student[6] = newDebugMsg.value_7;
m_debug_values_student[7] = newDebugMsg.value_8;
m_debug_values_student[8] = newDebugMsg.value_9;
m_debug_values_student[9] = newDebugMsg.value_10;
}
// > For the State Estimate from the Crazyflie
void cfStateEstimateCallback(const FlyingVehicleState & newStateEstimate)
{
// > For (x,y,z) positions
m_cf_state_estimate_xyz_rpy[0] = newStateEstimate.x;
m_cf_state_estimate_xyz_rpy[1] = newStateEstimate.y;
m_cf_state_estimate_xyz_rpy[2] = newStateEstimate.z;
// > For (roll,pitch,yaw) orientation
m_cf_state_estimate_xyz_rpy[3] = newStateEstimate.roll;
m_cf_state_estimate_xyz_rpy[4] = newStateEstimate.pitch;
m_cf_state_estimate_xyz_rpy[5] = newStateEstimate.yaw;
}
......@@ -114,8 +141,8 @@ void studentControllerSetpointChangedCallback(const SetpointWithHeader& newSetpo
// SERVICE CALLBACK FOR PROVIDING STATUS TO THE WEB INTERFACE
bool statusForWebInterfaceCallback(IntStringService::Request &request, IntStringService::Response &response)
{
// Get the statuses as string
str::string crazyradio_status_string;
// Get the CrazyRadio status as a string
std::string crazyradio_status_string;
switch (m_crazyradio_status)
{
case CRAZY_RADIO_STATE_CONNECTED:
......@@ -140,11 +167,248 @@ bool statusForWebInterfaceCallback(IntStringService::Request &request, IntString
}
}
// Get the Battery level as a string
std::string battery_level_string;
switch (m_battery_level)
{
case BATTERY_LEVEL_000:
{
battery_level_string = "000";
break;
}
case BATTERY_LEVEL_010:
{
battery_level_string = "010";
break;
}
case BATTERY_LEVEL_020:
{
battery_level_string = "020";
break;
}
case BATTERY_LEVEL_030:
{
battery_level_string = "030";
break;
}
case BATTERY_LEVEL_040:
{
battery_level_string = "040";
break;
}
case BATTERY_LEVEL_050:
{
battery_level_string = "050";
break;
}
case BATTERY_LEVEL_060:
{
battery_level_string = "060";
break;
}
case BATTERY_LEVEL_070:
{
battery_level_string = "070";
break;
}
case BATTERY_LEVEL_080:
{
battery_level_string = "080";
break;
}
case BATTERY_LEVEL_090:
{
battery_level_string = "090";
break;
}
case BATTERY_LEVEL_100:
{
battery_level_string = "100";
break;
}
case BATTERY_LEVEL_UNAVAILABLE:
{
battery_level_string = "unavailable";
break;
}
default:
{
battery_level_string = "unavailable";
break;
}
}
// Get the Flying status as a string
std::string flying_state_string;
switch (m_agent_operating_state)
{
case STATE_MOTORS_OFF:
{
flying_state_string = "motorsoff";
break;
}
case STATE_TAKE_OFF:
{
flying_state_string = "takeoff";
break;
}
case STATE_FLYING:
{
flying_state_string = "flying";
break;
}
case STATE_LAND:
{
flying_state_string = "land";
break;
}
case STATE_UNAVAILABLE:
{
flying_state_string = "unavailable";
break;
}
default:
{
flying_state_string = "unavailable";
break;
}
}
// Get the Flying status as a string
std::string instant_controller_string;
switch (m_instant_controller)
{
case DEFAULT_CONTROLLER:
{
instant_controller_string = "default";
break;
}
case DEMO_CONTROLLER:
{
instant_controller_string = "demo";
break;
}
case STUDENT_CONTROLLER:
{
instant_controller_string = "student";
break;
}
case MPC_CONTROLLER:
{
instant_controller_string = "mpc";
break;
}
case REMOTE_CONTROLLER:
{
instant_controller_string = "remote";
break;
}
case TUNING_CONTROLLER:
{
instant_controller_string = "tuning";
break;
}
case PICKER_CONTROLLER:
{
instant_controller_string = "picker";
break;
}
case TEMPLATE_CONTROLLER:
{
instant_controller_string = "template";
break;
}
case CSONE_CONTROLLER:
{
instant_controller_string = "csone";
break;
}
case TESTMOTORS_CONTROLLER:
{
instant_controller_string = "testmotors";
break;
}
default:
{
instant_controller_string = "none";
break;
}
}
// Concatenate the json together using a string stream
std::stringstream ss;
ss << "{\u0022crazyradiostatus\u0022: \u0022" << crazyradio_status_string << "\u0022}";
//ss << R"({"crazyradiostatus": ")" << m_crazyradio_status << R"("})";
std::string s = ss.str();
ss << "{";
ss << "\u0022crazyradiostatus\u0022: \u0022" << crazyradio_status_string << "\u0022";
ss << " , ";
ss << "\u0022batterylevel\u0022: \u0022" << battery_level_string << "\u0022";
ss << " , ";
ss << "\u0022flyingstate\u0022: \u0022" << flying_state_string << "\u0022";
ss << " , ";
ss << "\u0022instantcontroller\u0022: \u0022" << instant_controller_string << "\u0022";
ss << " , ";
ss << "\u0022setpointdefault\u0022:";
ss << "{";
ss << "\u0022x\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_default[0];
ss << " , ";
ss << "\u0022y\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_default[1];
ss << " , ";
ss << "\u0022z\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_default[2];
ss << " , ";
ss << "\u0022yaw\u0022: " << std::setprecision(1) << std::fixed << m_setpoint_default[3]*RAD2DEG;
ss << "}";
ss << " , ";
ss << "\u0022setpointstudent\u0022:";
ss << "{";
ss << "\u0022x\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_student[0];
ss << " , ";
ss << "\u0022y\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_student[1];
ss << " , ";
ss << "\u0022z\u0022: " << std::setprecision(3) << std::fixed << m_setpoint_student[2];
ss << " , ";
ss << "\u0022yaw\u0022: " << std::setprecision(1) << std::fixed << m_setpoint_student[3]*RAD2DEG;
ss << "}";
ss << " , ";
ss << "\u0022stateestimate\u0022:";
ss << "{";
ss << "\u0022x\u0022: " << std::setprecision(3) << std::fixed << m_cf_state_estimate_xyz_rpy[0];
ss << " , ";
ss << "\u0022y\u0022: " << std::setprecision(3) << std::fixed << m_cf_state_estimate_xyz_rpy[1];
ss << " , ";
ss << "\u0022z\u0022: " << std::setprecision(3) << std::fixed << m_cf_state_estimate_xyz_rpy[2];
ss << " , ";
ss << "\u0022roll\u0022: " << std::setprecision(1) << std::fixed << m_cf_state_estimate_xyz_rpy[3]*RAD2DEG;
ss << " , ";
ss << "\u0022pitch\u0022: " << std::setprecision(1) << std::fixed << m_cf_state_estimate_xyz_rpy[4]*RAD2DEG;
ss << " , ";
ss << "\u0022yaw\u0022: " << std::setprecision(1) << std::fixed << m_cf_state_estimate_xyz_rpy[5]*RAD2DEG;
ss << "}";
ss << " , ";
ss << "\u0022debugvaluesstudent\u0022:";
ss << "{";
ss << "\u0022value1\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[0];
ss << " , ";
ss << "\u0022value2\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[1];
ss << " , ";
ss << "\u0022value3\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[2];
ss << " , ";
ss << "\u0022value4\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[3];
ss << " , ";
ss << "\u0022value5\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[4];
ss << " , ";
ss << "\u0022value6\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[5];
ss << " , ";
ss << "\u0022value7\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[6];
ss << " , ";
ss << "\u0022value8\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[7];
ss << " , ";
ss << "\u0022value9\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[8];
ss << " , ";
ss << "\u0022value10\u0022: " << std::setprecision(3) << std::fixed << m_debug_values_student[9];
ss << "}";
ss << "}";
//std::string s = ss.str();
// Put the string into the response
//response.data = "test of service for web interface";
//response.data = "{\"crazyradiostatus\": \"%d\"}", m_crazyradio_status;
......@@ -192,6 +456,13 @@ int main(int argc, char* argv[])
// > Subscribe to the topic
ros::Subscriber crazyRadioStatusSubscriber = nodeHandle_to_crazyradio.subscribe("CrazyRadioStatus", 1, crazyRadioStatusCallback);
// FOR THE BATTERY LEVEL
// > Get a node handle to the Battery Monitor
std::string namespace_to_BatteryMonitor = m_namespace + "/BatteryMonitor";
ros::NodeHandle nodeHandle_to_BatteryMonitor(namespace_to_BatteryMonitor);
// > Subscribe to the topic
ros::Subscriber newBatteryLevelSubscriber = nodeHandle_to_BatteryMonitor.subscribe("Level", 1, newBatteryLevelCallback);
// FOR THE FLYING STATE OF THE AGENT
// > Get a node handle to the Flying Agent Client
std::string namespace_to_FlyingAgentClient = m_namespace + "/FlyingAgentClient";
......@@ -203,13 +474,6 @@ int main(int argc, char* argv[])
// > Subscribe to the topic
ros::Subscriber instantControllerSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("ControllerUsed", 1, instantControllerChangedCallback);
// FOR THE BATTERY LEVEL
// > Get a node handle to the Battery Monitor
std::string namespace_to_BatteryMonitor = m_namespace + "/BatteryMonitor";
ros::NodeHandle nodeHandle_to_BatteryMonitor(namespace_to_BatteryMonitor);
// > Subscribe to the topic
ros::Subscriber newBatteryLevelSubscriber = nodeHandle_to_BatteryMonitor.subscribe("Level", 1, newBatteryLevelCallback);
// FOR THE DEFAULT CONTROLLER SETPOINT
// > Get a node handle to the Default Controller Service
std::string namespace_to_DefaultController = m_namespace + "/DefaultControllerService";
......@@ -222,7 +486,18 @@ int main(int argc, char* argv[])
std::string namespace_to_StudentController = m_namespace + "/StudentControllerService";
ros::NodeHandle nodeHandle_to_StudentController(namespace_to_StudentController);
// > Subscribe to the topic
ros::Subscriber stuentControllerSetpointChangedSubscriber = nodeHandle_to_StudentController.subscribe("SetpointChanged", 1, studentControllerSetpointChangedCallback);
ros::Subscriber studentControllerSetpointChangedSubscriber = nodeHandle_to_StudentController.subscribe("SetpointChanged", 1, studentControllerSetpointChangedCallback);
// FOR THE DEBUG VALUES OF THE STUDENT CONTROLLER
// > Subscribe to the topic
ros::Subscriber studentControllerDebugValuesSubscriber = nodeHandle_to_StudentController.subscribe("DebugTopic", 1, studentControllerDebugValuesCallback);
// FOR THE STATE ESTIMATE FROM THE CRAZYFLIE
// > Get a node handle to the Crazy Radio node
std::string namespace_to_CrazyRadio = m_namespace + "/CrazyRadio";
ros::NodeHandle nodeHandle_to_CrazyRadio(namespace_to_CrazyRadio);
// > Subscribe to the topic
ros::Subscriber cfStateEstimateSubscriber = nodeHandle_to_CrazyRadio.subscribe("CFStateEstimate", 1, cfStateEstimateCallback);
......
......@@ -305,6 +305,24 @@ void timerCallback_update_cfStateEstimate(const ros::TimerEvent&)
// Simulate the quadrotor for one time step
m_quadrotor_sim.simulate_for_one_time_step( yaml_cfSimulation_deltaT_in_seconds );
// Perform safety check if required
if (yaml_isEnabled_strictSafety)
{
// Estimate the height at the next measurement
float height_at_next_measurement = m_quadrotor_sim.m_position[2] + yaml_cfSimulation_deltaT_in_seconds * m_quadrotor_sim.m_velocity[2];
// Turn-off if too high
if (height_at_next_measurement > yaml_maxHeight_for_strictSafety_meters)
{
// Send the MOTORS-OFF command to the Flying Agent Client
IntWithHeader msg;
msg.shouldCheckForAgentID = false;
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
flyingAgentClientCommandPublisher.publish(msg);
// Inform the user
ROS_ERROR_STREAM("[CRAZY RADIO] Height safety check failed, measured = " << height_at_next_measurement << ", max allowed = " << yaml_maxHeight_for_strictSafety_meters );
}
}
// Local variable for the data of this quadrotor
FlyingVehicleState quadrotor_data;
......@@ -467,6 +485,11 @@ void fetchCrazyRadioConfigYamlParameters(ros::NodeHandle& nodeHandle)
// Add the "CrazyRadioConfig" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "CrazyRadioConfig");
// FLAG FOR WHETHER TO USE HEIGHT AS A TRIGGER FOR
// PUBLISHING A MOTORS-OFF COMMAND
yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
yaml_maxHeight_for_strictSafety_meters = getParameterFloat(nodeHandle_for_paramaters,"maxHeight_for_strictSafety_meters");
// SIMULATION FREQUENCY FOR EMULATING A CRAZYFLIE [Hertz]
float yaml_cfSimulation_frequency = getParameterFloat(nodeHandle_for_paramaters,"cfSimulation_frequency");
......
......@@ -452,7 +452,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
// type "float64" that you can fill in with data you would like to plot in
// real-time.
// debugMsg.value_1 = thrustAdjustment;
debugMsg.value_1 = thrustAdjustment;
debugMsg.value_2 = rollRate_forResponse;
debugMsg.value_3 = pitchRate_forResponse;
debugMsg.value_4 = yawRate_forResponse;
// ......................
// debugMsg.value_10 = your_variable_name;
......@@ -639,25 +642,6 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
return true;
}
// --------------------------------------------------------------------------------
// DDDD EEEEE BBBB U U GGGG V V A L U U EEEEE SSSS
// D D E B B U U G V V A A L U U E S
// D D EEE BBBB U U G GG V V A A L U U EEE SSS
// D D E B B U U G G V V AAAAA L U U E S
// DDDD EEEEE BBBB UUU GGG V A A LLLLL UUU EEEEE SSSS
//
// CCCC A L L BBBB A CCCC K K
// C A A L L B B A A C K K
// C A A L L BBBB A A C KKK
// C AAAAA L L B B AAAAA C K K
// CCCC A A LLLLL LLLLL BBBB A A CCCC K K
// --------------------------------------------------------------------------------
bool getDebugValuesCallback(GetDebugValuesService::Request &request, GetDebugValuesService::Response &response)
{
// What should I write in this function? Or do the students fill it in depending on what they want to debug?
}
......@@ -1101,14 +1085,6 @@ int main(int argc, char* argv[]) {
// Instantiate the local variable "getDebugValuesService" to be
// a "ros::ServiceServer" type variable that advertises the service
// called "GetDebugValues". This service has the input-output
// behaviour defined in the "GetDebugValuesService.srv" file (located
// in the "srv" folder). When a request is made