Commit 1e87768d authored by roangel's avatar roangel
Browse files

Need to test downstairs

parent 7e5b6f6b
......@@ -84,6 +84,8 @@ private:
std::string m_ros_namespace;
ros::Timer m_timer_yaml_file;
int m_student_id;
CrazyflieContext m_context;
......@@ -108,6 +110,7 @@ private:
void flyingStateChangedCallback(const std_msgs::Int32& msg);
void setpointCallback(const Setpoint& newSetpoint);
void DBChangedCallback(const std_msgs::Int32& msg);
void yamlFileTimerCallback(const ros::TimerEvent&);
float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage);
......
......@@ -67,6 +67,19 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
qstr.append(QString::fromStdString(m_context.crazyflieName));
ui->groupBox->setTitle(qstr);
std::vector<float> default_setpoint(4);
ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
if(!nh_safeControllerService.getParam("defaultSetPoint", default_setpoint))
{
ROS_ERROR_STREAM("couldn't find parameter 'defaultSetPoint'");
}
ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));
disableGUI();
}
......@@ -254,15 +267,15 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 3));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 3));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 2));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 2));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 2));
// also update diff
ui->diff_x->setText(QString::number(m_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 3));
ui->diff_yaw->setText(QString::number((m_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 2));
}
}
}
......@@ -315,28 +328,38 @@ void MainWindow::on_pushButton_3_clicked()
ROS_INFO("command disconnect published");
}
void MainWindow::on_load_yaml_button_clicked()
void MainWindow::yamlFileTimerCallback(const ros::TimerEvent&)
{
std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
ROS_INFO_STREAM(d_fall_pps_path);
// first, reload the name of the custom controller:
std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
system(cmd.c_str());
ROS_INFO_STREAM(cmd);
// then, reload the parameters of the custom controller:
cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomControllerService";
system(cmd.c_str());
ROS_INFO_STREAM(cmd);
ui->load_yaml_button->setEnabled(true);
}
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
void MainWindow::on_load_yaml_button_clicked()
{
if(!nodeHandle.getParam(name, val))
{
ROS_ERROR_STREAM("missing parameter '" << name << "'");
}
if(val.size() != length)
{
ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
}
ros::NodeHandle nodeHandle("~");
m_timer_yaml_file = nodeHandle.createTimer(ros::Duration(0.5), &MainWindow::yamlFileTimerCallback, this, true);
ui->load_yaml_button->setEnabled(false);
}
void MainWindow::on_en_custom_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_CUSTOM_CONTROLLER;
// this->PPSClientCommandPublisher.publish(msg);
this->PPSClientCommandPublisher.publish(msg);
}
......@@ -344,19 +367,5 @@ void MainWindow::on_en_safe_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_SAFE_CONTROLLER;
std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
ROS_INFO_STREAM(d_fall_pps_path);
std::string cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeControllerService";
system(cmd.c_str());
ROS_INFO_STREAM(cmd);
std::vector<float> setpoint(4);
ros::NodeHandle nodeHandle(m_ros_namespace + "/SafeControllerService");
loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
ROS_INFO_STREAM("setpoint z:" << setpoint[2]);
// this->PPSClientCommandPublisher.publish(msg);
this->PPSClientCommandPublisher.publish(msg);
}
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
<!-- Written by QtCreator 4.0.2, 2017-08-29T18:10:33. -->
<!-- Written by QtCreator 4.0.2, 2017-09-01T14:47:27. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
......
......@@ -223,16 +223,6 @@ void landCF(CrazyflieData& current_local_coordinates)
loadSafeController();
}
void goToOrigin()
{
Setpoint setpoint_msg;
setpoint_msg.x = 0;
setpoint_msg.y = 0;
setpoint_msg.z = 0.4;
setpoint_msg.yaw = 0;
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
}
void changeFlyingStateTo(int new_state)
{
if(crazyradio_status == CONNECTED)
......@@ -311,7 +301,6 @@ void viconCallback(const ViconData& viconData) {
{
changed_state_flag = false;
// need to change setpoint to the one from file
goToOrigin();
ROS_INFO("STATE_FLYING");
}
break;
......
......@@ -49,6 +49,7 @@ std::vector<float> estimatorMatrix(2);
float prevEstimate[9];
std::vector<float> setpoint(4);
std::vector<float> defaultSetpoint(4);
float saturationThrust;
CrazyflieData previousLocation;
......@@ -82,7 +83,7 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
loadParameterFloatVector(nodeHandle, "defaultSetpoint", defaultSetpoint, 4);
}
float computeMotorPolyBackward(float thrust) {
......@@ -159,7 +160,11 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
state[8] = est[8];
}
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
CrazyflieData vicon = request.ownCrazyflie;
float yaw_measured = request.ownCrazyflie.yaw;
......@@ -249,7 +254,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
previousLocation = request.ownCrazyflie;
bag.write("ControlOutput", ros::Time::now(), response.controlOutput);
return true;
}
......@@ -268,6 +273,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle nodeHandle("~");
loadParameters(nodeHandle);
setpoint = defaultSetpoint; // only first time setpoint is equal to default setpoint
ros::Publisher setpointPublisher = nodeHandle.advertise<Setpoint>("Setpoint", 1);
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
......
Supports Markdown
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