From 7e5b6f6bdeaa324c5f10ec9b220766b14c78150d Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Fri, 1 Sep 2017 11:56:59 +0200
Subject: [PATCH] got to reload config files finally. Need to extend it to the
whole system
---
.../GUI_Qt/studentGUI/include/MainWindow.h | 2 +
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 40 ++++++++++++++++---
2 files changed, 36 insertions(+), 6 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index 3a4895d9..b3aa5eeb 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -82,6 +82,8 @@ private:
float m_battery_voltage;
int m_battery_level;
+ std::string m_ros_namespace;
+
int m_student_id;
CrazyflieContext m_context;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index b7637420..8eacfed7 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -4,6 +4,7 @@
#include <ros/ros.h>
#include <ros/network.h>
+#include <ros/package.h>
#include "d_fall_pps/CMQuery.h"
@@ -22,13 +23,13 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setCrazyRadioStatus(DISCONNECTED);
- std::string ros_namespace = ros::this_node::getNamespace();
- ROS_INFO("namespace: %s", ros_namespace.c_str());
+ m_ros_namespace = ros::this_node::getNamespace();
+ ROS_INFO("namespace: %s", m_ros_namespace.c_str());
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
- ros::NodeHandle nodeHandle(ros_namespace);
+ ros::NodeHandle nodeHandle(m_ros_namespace);
// subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
@@ -43,7 +44,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
- ros::NodeHandle nh_PPSClient(ros_namespace + "/PPSClient");
+ ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
@@ -319,16 +320,43 @@ void MainWindow::on_load_yaml_button_clicked()
}
+void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ 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");
+ }
+}
+
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);
}
+
void MainWindow::on_en_safe_controller_clicked()
{
std_msgs::Int32 msg;
msg.data = CMD_USE_SAFE_CONTROLLER;
- this->PPSClientCommandPublisher.publish(msg);
+
+
+ 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);
}
--
GitLab