Commit 13d6ffd6 authored by roangel's avatar roangel
Browse files

added a different link to set the setpoint for GUI, so that PPSClient decides...

added a different link to set the setpoint for GUI, so that PPSClient decides in the end which setpoint to use. Difference between high-level setpoint and low-level setpoint
parent 70ad70b2
......@@ -98,7 +98,7 @@ private:
ros::Subscriber CFBatterySubscriber;
ros::Subscriber flyingStateSubscriber;
ros::Publisher setpointPublisher;
ros::Publisher controllerSetpointPublisher;
ros::Subscriber setpointSubscriber;
ros::Subscriber DBChangedSubscriber;
......
......@@ -38,11 +38,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
setpointPublisher = nodeHandle.advertise<Setpoint>("SafeControllerService/Setpoint", 1);
setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
ros::NodeHandle my_nodeHandle("~");
controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);
......@@ -325,7 +326,7 @@ void MainWindow::on_set_setpoint_button_clicked()
msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
this->setpointPublisher.publish(msg_setpoint);
this->controllerSetpointPublisher.publish(msg_setpoint);
}
void MainWindow::on_pushButton_3_clicked()
......
......@@ -326,6 +326,13 @@
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QPushButton" name="load_safe_yaml_button">
<property name="text">
<string>Load YAML file</string>
</property>
</widget>
</item>
</layout>
</widget>
<widget class="QWidget" name="tab_4">
......
......@@ -77,6 +77,8 @@ ros::ServiceClient customController;
bool strictSafety;
float angleMargin;
Setpoint controller_setpoint;
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
......@@ -260,25 +262,24 @@ void landTimerCallback(const ros::TimerEvent&)
finished_land = true;
}
void goToDefaultSetpoint()
void goToControllerSetpoint()
{
std::vector<float> default_setpoint(4);
ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
{
ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
}
Setpoint setpoint_msg;
setpoint_msg.x = default_setpoint[0];
setpoint_msg.y = default_setpoint[1];
setpoint_msg.z = default_setpoint[2];
ROS_INFO_STREAM("Z =" << default_setpoint[2]);
setpoint_msg.yaw = default_setpoint[3];
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
// std::vector<float> default_setpoint(4);
// ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
// ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
// if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
// {
// ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
// }
// Setpoint setpoint_msg;
// setpoint_msg.y = default_setpoint[1];
// setpoint_msg.z = default_setpoint[2];
// ROS_INFO_STREAM("Z =" << default_setpoint[2]);
// setpoint_msg.yaw = default_setpoint[3];
safeControllerServiceSetpointPublisher.publish(controller_setpoint);
}
//is called when new data from Vicon arrives
......@@ -323,8 +324,8 @@ void viconCallback(const ViconData& viconData) {
if(changed_state_flag) // stuff that will be run only once when changing state
{
changed_state_flag = false;
// need to change setpoint to the one from file
goToDefaultSetpoint();
// need to change setpoint to the controller one
goToControllerSetpoint();
ROS_INFO("STATE_FLYING");
}
break;
......@@ -502,12 +503,41 @@ void crazyRadioStatusCallback(const std_msgs::Int32& msg)
crazyradio_status = msg.data;
}
void controllerSetPointCallback(const Setpoint& newSetpoint)
{
// load in variable the setpoint
controller_setpoint = newSetpoint;
// if we are in flying, set it up NOW
if(flying_state == STATE_FLYING)
{
goToControllerSetpoint();
}
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
ros_namespace = ros::this_node::getNamespace();
// load default setpoint
std::vector<float> default_setpoint(4);
ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
{
ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
}
controller_setpoint.x = default_setpoint[0];
controller_setpoint.y = default_setpoint[1];
controller_setpoint.z = default_setpoint[2];
controller_setpoint.yaw = default_setpoint[3];
// load context parameters
loadParameters(nodeHandle);
//ros::service::waitForService("/CentralManagerService/CentralManager");
......@@ -542,6 +572,7 @@ int main(int argc, char* argv[])
// SafeControllerServicePublisher:
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
// subscriber for DBChanged
ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);
......
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