Commit e8406d2b authored by beuchatp's avatar beuchatp
Browse files

Adjust the size of the ViconData and CFOnboardEstimate queues to avoid...

Adjust the size of the ViconData and CFOnboardEstimate queues to avoid processing old data (this may be the reason that the CF flys too high on first take-off when using CF onboard estimates
parent 23237f88
......@@ -423,7 +423,7 @@ add_executable(TemplateControllerService src/nodes/TemplateControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(TestMotorsControllerService src/nodes/TestMotorsControllerService.cpp
src/classes/GetParamtersAndNamespaces.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/classes/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
add_executable(MocapEmulator src/nodes/MocapEmulator.cpp
......
......@@ -146,6 +146,12 @@ private:
// Sound effect for when the controller changes while flying
//QSoundEffect m_soundEffect_controllerChanged;
// FLAG FOR WHAT DATA TO EMIT
// > For "poseDataMocap"
bool m_shouldEmitPoseDataMocap = true;
// > For "poseDataOnboard"
bool m_shouldEmitPoseDataOnboard = false;
#ifdef CATKIN_MAKE
// --------------------------------------------------- //
......
......@@ -202,6 +202,28 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
// GET THE "onload" PARAMETERS FOR WHAT DATA TO EMIT
// > Create a "ros::NodeHandle" type local variable
// "nodeHandle" as the current node, the "~" indcates
// that "self" is the node handle assigned to this
// variable.
ros::NodeHandle nodeHandle("~");
// > For "poseDataMocap"
if(!nodeHandle.getParam("onlaunch_shouldEmitPoseDataMocap", m_shouldEmitPoseDataMocap))
{
m_shouldEmitPoseDataMocap = false;
ROS_ERROR("[CONTROLLER TABS GUI] missing parameter 'onlaunch_shouldEmitPoseDataMocap'");
}
// > For "poseDataOnboard"
if(!nodeHandle.getParam("onlaunch_shouldEmitPoseDataOnboard", m_shouldEmitPoseDataOnboard))
{
m_shouldEmitPoseDataOnboard = false;
ROS_ERROR("[CONTROLLER TABS GUI] missing parameter 'onlaunch_shouldEmitPoseDataOnboard'");
}
// CREATE A NODE HANDLE TO THIS GUI
ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
......@@ -209,29 +231,39 @@ ControllerTabs::ControllerTabs(QWidget *parent) :
ros::NodeHandle nodeHandle_dfall_root("/dfall");
// CREATE THE SUBSCRIBER TO THE MOTION CAPTURE DATA
m_poseDataMocapSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 100, &ControllerTabs::poseDataMocapReceivedCallback, this);
m_poseDataMocapSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 3, &ControllerTabs::poseDataMocapReceivedCallback, this);
// CREATE THE SUBSCRIBER TO AGENT SPECIFIC TOPICS:
// i.e., only if this is an agent GUI
if (m_type == TYPE_AGENT)
{
// THE STATE ESTIMATE FROM ONBOARD THE FLYING AGENT
m_poseDataOnboardSubscriber = nodeHandle_for_this_gui.subscribe("CrazyRadio/CFStateEstimate", 20, &ControllerTabs::poseDataOnboardReceivedCallback, this);
m_poseDataOnboardSubscriber = nodeHandle_for_this_gui.subscribe("CrazyRadio/CFStateEstimate", 3, &ControllerTabs::poseDataOnboardReceivedCallback, this);
// THE CONTROLLER THAT IS CURRENTLY OPERATING
controllerUsedSubscriber = nodeHandle_for_this_gui.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
}
// NOTE: FOR THE "poseDataMocap" AND "poseDataOnboard"
// SUBSCRIBERS:
// > The second argument is the queue_size, and the
// documentation states that:
// "If messages are arriving too fast and you are
// unable to keep up, roscpp will start throwing
// away messages."
// > It is clearly stated that publisher queues throw
// away old messages first, and subscriber queues
// are likely the same.
// > As we are only interested in latest measurement
// a short queue is used.
// CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
ros::NodeHandle dfall_root_nodeHandle("/dfall");
// > Publisher for the emergency stop button
//emergencyStopPublisher = dfall_root_nodeHandle.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1);
//emergencyStopPublisher = nodeHandle_dfall_root.advertise<dfall_pkg::IntWithHeader>("EmergencyStop", 1);
// > For changes in the database that defines {agentID,cfID,flying zone} links
//databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
//databaseChangedSubscriber = nodeHandle_dfall_root.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
centralManagerDatabaseService = nodeHandle_dfall_root.serviceClient<dfall_pkg::CMQuery>("CentralManagerService/Query", false);
#endif
......@@ -390,15 +422,18 @@ void ControllerTabs::poseDataMocapReceivedCallback(const dfall_pkg::ViconData& v
// float originZ = (area.zmin + area.zmax) / 2.0;
//pose_in_global_frame.z -= originZ;
// emit measuredPoseValueChanged(
// pose_in_global_frame.x,
// pose_in_global_frame.y,
// pose_in_global_frame.z,
// pose_in_global_frame.roll,
// pose_in_global_frame.pitch,
// pose_in_global_frame.yaw,
// pose_in_global_frame.isValid
// );
if (m_shouldEmitPoseDataMocap)
{
emit measuredPoseValueChanged(
pose_in_global_frame.x,
pose_in_global_frame.y,
pose_in_global_frame.z,
pose_in_global_frame.roll,
pose_in_global_frame.pitch,
pose_in_global_frame.yaw,
pose_in_global_frame.isValid
);
}
}
}
}
......@@ -415,8 +450,8 @@ void ControllerTabs::poseDataMocapReceivedCallback(const dfall_pkg::ViconData& v
// "m_poseDataOnboardSubscriber"
void ControllerTabs::poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehicleState& pose_in_local_frame)
{
//if(pose_in_local_frame.vehicleName == m_object_name_for_emitting_pose_data)
//{
if (m_shouldEmitPoseDataOnboard)
{
emit measuredPoseValueChanged(
pose_in_local_frame.x,
pose_in_local_frame.y,
......@@ -426,7 +461,7 @@ void ControllerTabs::poseDataOnboardReceivedCallback(const dfall_pkg::FlyingVehi
pose_in_local_frame.yaw,
pose_in_local_frame.isValid
);
//}
}
}
#endif
......@@ -675,7 +710,7 @@ void ControllerTabs::setAgentIDsToCoordinate(QVector<int> agentIDs , bool should
m_change_highlighted_controller_mutex.lock();
// > For receiving the state estimate from onboard the fying agent
m_poseDataOnboardSubscriber = agent_base_nodeHandle.subscribe("CrazyRadio/CFStateEstimate", 20, &ControllerTabs::poseDataOnboardReceivedCallback, this);
m_poseDataOnboardSubscriber = agent_base_nodeHandle.subscribe("CrazyRadio/CFStateEstimate", 3, &ControllerTabs::poseDataOnboardReceivedCallback, this);
// > For receiving messages that the instant controller was changed
controllerUsedSubscriber = agent_base_nodeHandle.subscribe("FlyingAgentClient/ControllerUsed", 1, &ControllerTabs::controllerUsedChangedCallback, this);
m_change_highlighted_controller_mutex.unlock();
......
......@@ -82,7 +82,7 @@ bool rosNodeThread::init()
ros::Time::init();
ros::NodeHandle nh("~");
//m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
//m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 3, &rosNodeThread::messageCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
......
......@@ -61,7 +61,7 @@
#include "dfall_pkg/CMCommand.h"
// Include other classes
#include "CrazyflieIO.h"
#include "classes/CrazyflieIO.h"
......
......@@ -196,8 +196,25 @@
output = "screen"
type = "FlyingAgentGUI"
>
<param name="type" type="str" value="agent" />
<param name="agentID" value="$(arg agentID)" />
<param
name="type"
type="str"
value="agent"
/>
<param
name="agentID"
value="$(arg agentID)"
/>
<param
name="onlaunch_shouldEmitPoseDataMocap"
type="bool"
value="true"
/>
<param
name="onlaunch_shouldEmitPoseDataOnboard"
type="bool"
value="false"
/>
</node>
</group>
......
......@@ -36,7 +36,7 @@
// ----------------------------------------------------------------------------------
#include "CrazyflieIO.h"
#include "classes/CrazyflieIO.h"
#include <stdlib.h>
#include <ros/ros.h>
#include <ros/package.h>
......
......@@ -1539,9 +1539,17 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
{
// > Create a node handle to the root of the D-FaLL system
ros::NodeHandle nodeHandle_dfall_root("/dfall");
// > keeps 50 messages because otherwise the next message may
// overwrite the data while the current message is been used
viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 50, viconCallback);
// > The second argument is the queue_size, and the
// documentation states that:
// "If messages are arriving too fast and you are
// unable to keep up, roscpp will start throwing
// away messages."
// > It is clearly stated that publisher queues throw
// away old messages first, and subscriber queues
// are likely the same.
// > As we are only interested in latest measurement
// a short queue is used.
viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 3, viconCallback);
}
else
{
......@@ -1555,9 +1563,17 @@ void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
std::string m_namespace = ros::this_node::getNamespace();
std::string namespace_to_crazy_radio = m_namespace + "/CrazyRadio";
ros::NodeHandle nodeHandle_to_crazy_radio(namespace_to_crazy_radio);
// > keeps 20 messages because otherwise the next message may
// overwrite the data while the current message is been used
onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 20, onboardEstimateCallback);
// > The second argument is the queue_size, and the
// documentation states that:
// "If messages are arriving too fast and you are
// unable to keep up, roscpp will start throwing
// away messages."
// > It is clearly stated that publisher queues throw
// away old messages first, and subscriber queues
// are likely the same.
// > As we are only interested in latest measurement
// a short queue is used.
onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 3, onboardEstimateCallback);
}
else
{
......@@ -1760,20 +1776,36 @@ int main(int argc, char* argv[])
// LOCALISATION DATA FROM MOTION CAPTURE SYSTEM
// > keeps 50 messages because otherwise the next message may
// overwrite the data while the current message is been used
// > The second argument is the queue_size, and the
// documentation states that:
// "If messages are arriving too fast and you are
// unable to keep up, roscpp will start throwing
// away messages."
// > It is clearly stated that publisher queues throw
// away old messages first, and subscriber queues
// are likely the same.
// > As we are only interested in latest measurement
// a short queue is used.
if (yaml_shouldUse_mocapMeasurements)
{
viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 50, viconCallback);
viconSubscriber = nodeHandle_dfall_root.subscribe("ViconDataPublisher/ViconData", 3, viconCallback);
}
// LOCALISATION DATA FROM ONBOARD THE FLYING VEHICLE
// > keeps 20 messages because otherwise the next message may
// overwrite the data while the current message is been used
// > The second argument is the queue_size, and the
// documentation states that:
// "If messages are arriving too fast and you are
// unable to keep up, roscpp will start throwing
// away messages."
// > It is clearly stated that publisher queues throw
// away old messages first, and subscriber queues
// are likely the same.
// > As we are only interested in latest measurement
// a short queue is used.
if (yaml_shouldUse_onboardEstimate)
{
onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 20, onboardEstimateCallback);
onboardEstimateSubscriber = nodeHandle_to_crazy_radio.subscribe("CFStateEstimate", 3, onboardEstimateCallback);
}
......
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