diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
index 0527a979885d05a812381e59dfc1a33b3e651d34..e12164b6614f43365c12c1d74b3e03eb1e6df544 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/pickercontrollertab.ui
@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>1978</width>
- <height>1188</height>
+ <height>1312</height>
</rect>
</property>
<property name="font">
@@ -2389,124 +2389,304 @@
</widget>
</item>
<item row="22" column="4">
- <widget class="QLineEdit" name="lineEdit_measured_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ <layout class="QHBoxLayout" name="horizontalLayout_20">
+ <property name="spacing">
+ <number>0</number>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_x_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="22" column="8">
+ <layout class="QHBoxLayout" name="horizontalLayout_36">
+ <property name="spacing">
+ <number>0</number>
</property>
- </widget>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_yaw_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
<item row="22" column="7">
- <widget class="QLineEdit" name="lineEdit_measured_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
+ <layout class="QHBoxLayout" name="horizontalLayout_37">
+ <property name="spacing">
+ <number>0</number>
</property>
- </widget>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_z_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
<item row="22" column="6">
- <widget class="QLineEdit" name="lineEdit_measured_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="22" column="8">
- <widget class="QLineEdit" name="lineEdit_measured_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
+ <layout class="QHBoxLayout" name="horizontalLayout_38">
+ <property name="spacing">
+ <number>0</number>
</property>
- </widget>
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="frame_y_unavailable">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_5">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
</layout>
</item>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
index 1a49ebd63f46be84b626c67b5b9fcfc4030287d0..0e428a13ebfb6c0c37541ecf33f7888e95119c27 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/pickercontrollertab.h
@@ -2,7 +2,9 @@
#define PICKERCONTROLLERTAB_H
#include <QWidget>
+#include <QMutex>
#include <QVector>
+#include <QTextStream>
#define DECIMAL_PLACES_POSITION 2
#define DECIMAL_PLACES_ANGLE_DEGREES 1
@@ -11,13 +13,13 @@
#define DEFAULT_INCREMENT_POSITION_XY 0.01
#define DEFAULT_INCREMENT_POSITION_Z 0.01
#define DEFAULT_INCREMENT_ANGLE_DEGREES 5
-#define DEFAULT_INCREMENT_MASS_GRAMS 1
+#define DEFAULT_INCREMENT_MASS_GRAMS 0.5
#define PICKER_STATE_UNKNOWN -1
#define PICKER_STATE_STANDBY 0
#define PICKER_STATE_GOTO_START 1
#define PICKER_STATE_ATTACH 2
-#define PICKER_STATE_LIFT 3
+#define PICKER_STATE_LIFT_UP 3
#define PICKER_STATE_GOTO_END 4
#define PICKER_STATE_PUT_DOWN 5
#define PICKER_STATE_SQUAT 6
@@ -29,6 +31,36 @@
#define PICKER_DEFAULT_YAW_DEGREES 0
#define PICKER_DEFAULT_MASS_GRAMS 30
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.h>
+
+// Include the standard message types
+//#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+// Include the DFALL message types
+//#include "d_fall_pps/IntWithHeader.h"
+#include "d_fall_pps/SetpointWithHeader.h"
+//#include "d_fall_pps/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "d_fall_pps/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace d_fall_pps;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
namespace Ui {
@@ -43,12 +75,14 @@ public:
explicit PickerControllerTab(QWidget *parent = 0);
~PickerControllerTab();
-private:
- int current_picker_state = PICKER_STATE_STANDBY;
- void publish_setpoint_if_current_state_matches(QVector<int> state_to_match);
- void publish_request_setpoint_change_for_state(int state_to_publish);
+public slots:
+ void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
+ void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+ void poseDataUnavailableSlot();
+
+
private slots:
void on_button_goto_start_clicked();
@@ -201,8 +235,58 @@ private slots:
void on_lineEdit_increment_mass_editingFinished();
+
+
private:
Ui::PickerControllerTab *ui;
+
+ // --------------------------------------------------- //
+ // PRIVATE VARIABLES
+
+ // The type of this node, i.e., agent or a coordinator,
+ // specified as a parameter in the "*.launch" file
+ int m_type = 0;
+
+ // The ID of this node
+ int m_ID;
+
+ // For coordinating multiple agents
+ std::vector<int> m_vector_of_agentIDs_toCoordinate;
+ bool m_shouldCoordinateAll = true;
+ QMutex m_agentIDs_toCoordinate_mutex;
+
+ // THE CURRENT STATE OF THE PICKER
+ int current_picker_state = PICKER_STATE_STANDBY;
+
+#ifdef CATKIN_MAKE
+ // PUBLISHER
+ // > For requesting the setpoint to be changed
+ ros::Publisher requestSetpointChangePublisher;
+
+ // SUBSCRIBER
+ // > For being notified when the setpoint is changed
+ ros::Subscriber setpointChangedSubscriber;
+#endif
+
+
+
+ // --------------------------------------------------- //
+ // PRIVATE FUNCTIONS
+
+#ifdef CATKIN_MAKE
+ // For receiving message that the setpoint was changed
+ void setpointChangedCallback(const d_fall_pps::SetpointWithHeader& newSetpoint);
+
+ // Fill the header for a message
+ void fillSetpointMessageHeader( d_fall_pps::SetpointWithHeader & msg );
+
+ // Get the paramters that specify the type and ID
+ bool getTypeAndIDParameters();
+#endif
+
+ void publish_setpoint_if_current_state_matches(QVector<int> state_to_match);
+
+ void publish_request_setpoint_change_for_state(int state_to_publish);
};
#endif // PICKERCONTROLLERTAB_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
index f8cc168eb603287f20cb93c6ada74fb995453d16..17b4ef6076e0128a165089bd2814c3b078bcdd46 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/pickercontrollertab.cpp
@@ -9,14 +9,15 @@ PickerControllerTab::PickerControllerTab(QWidget *parent) :
// HIDE ALL THE "GREEN FRAMES"
+ // > These indicate which state is currently active
ui->frame_goto_start_active->setVisible(false);
- ui->frame_attach_active->setVisible(false);
- ui->frame_lift_up_active->setVisible(false);
- ui->frame_goto_end_active->setVisible(false);
- ui->frame_put_down_active->setVisible(false);
- ui->frame_squat_active->setVisible(false);
- ui->frame_jump_active->setVisible(false);
- ui->frame_standby_active->setVisible(false);
+ ui->frame_attach_active ->setVisible(false);
+ ui->frame_lift_up_active ->setVisible(false);
+ ui->frame_goto_end_active ->setVisible(false);
+ ui->frame_put_down_active ->setVisible(false);
+ ui->frame_squat_active ->setVisible(false);
+ ui->frame_jump_active ->setVisible(false);
+ ui->frame_standby_active ->setVisible(false);
// SET DEFAULTS FOR THE INCREMENT
ui->lineEdit_increment_x ->setText(QString::number( DEFAULT_INCREMENT_POSITION_XY, 'f', DECIMAL_PLACES_POSITION));
@@ -38,6 +39,99 @@ PickerControllerTab::PickerControllerTab(QWidget *parent) :
// SET THE INITIAL CHECKBOX STATE FOR PUBLISHING EVERY CHANGED VALUE
ui->checkbox_should_publish_value_changed->setChecked(true);
+
+ // HIDE ALL THE "RED FRAMES"
+ // > These indicate when an occuled measurement is recieved
+ ui->frame_x_unavailable ->setVisible(false);
+ ui->frame_y_unavailable ->setVisible(false);
+ ui->frame_z_unavailable ->setVisible(false);
+ ui->frame_yaw_unavailable->setVisible(false);
+
+ // SET DEFAULTS FOR ALL THE {x,y,z,yaw,mass} LineEdits
+ // > For Goto Start
+ ui->lineEdit_goto_start_x ->setText(QString::number( PICKER_DEFAULT_X, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_start_y ->setText(QString::number( PICKER_DEFAULT_Y, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_start_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_start_yaw->setText(QString::number( PICKER_DEFAULT_YAW_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // > For Attach
+ ui->lineEdit_attach_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Lift Up
+ ui->lineEdit_lift_up_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_lift_up_mass ->setText(QString::number( PICKER_DEFAULT_MASS_GRAMS, 'f', DECIMAL_PLACES_MASS_GRAMS));
+ // > For Goto End
+ ui->lineEdit_goto_end_x ->setText(QString::number( PICKER_DEFAULT_X, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_end_y ->setText(QString::number( PICKER_DEFAULT_Y, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_goto_end_yaw ->setText(QString::number( PICKER_DEFAULT_YAW_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ // > For Put Down
+ ui->lineEdit_put_down_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Squat
+ ui->lineEdit_squat_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Jump
+ ui->lineEdit_jump_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ // > For Standby
+ ui->lineEdit_standby_x ->setText(QString::number( PICKER_DEFAULT_X, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_standby_y ->setText(QString::number( PICKER_DEFAULT_Y, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_standby_z ->setText(QString::number( PICKER_DEFAULT_Z, 'f', DECIMAL_PLACES_POSITION));
+ ui->lineEdit_standby_yaw ->setText(QString::number( PICKER_DEFAULT_YAW_DEGREES, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+ ui->lineEdit_standby_mass ->setText(QString::number( PICKER_DEFAULT_MASS_GRAMS, 'f', DECIMAL_PLACES_MASS_GRAMS));
+
+
+
+
+
+#ifdef CATKIN_MAKE
+
+ //ros::init();
+
+ // Get the namespace of this node
+ std::string this_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PICKER CONTROLLER TAB GUI] ros::this_node::getNamespace() = " << this_namespace);
+
+ // Get the type and ID of this flying agent GUI
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+
+ // Stall if the node IDs are not valid
+ if ( !isValid_type_and_ID )
+ {
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+
+
+ // CREATE A NODE HANDLE TO THIS GUI
+ ros::NodeHandle nodeHandle_for_this_gui(this_namespace);
+
+ // CREATE THE REQUEST SETPOINT CHANGE PUBLISHER
+ requestSetpointChangePublisher = nodeHandle_for_this_gui.advertise<d_fall_pps::SetpointWithHeader>("PickerControllerService/RequestSetpointChange", 1);
+
+ // SUBSCRIBE TO SETPOINT CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("PickerControllerService/SetpointChanged", 1, &PickerControllerTab::setpointChangedCallback, this);
+ }
+
+ // GET THE CURRENT SETPOINT
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = nodeHandle_for_this_gui.serviceClient<d_fall_pps::GetSetpointService>("PickerControllerService/GetCurrentSetpoint", false);
+ d_fall_pps::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[PICKER CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+ }
+
+#endif
}
PickerControllerTab::~PickerControllerTab()
@@ -109,7 +203,7 @@ void PickerControllerTab::publish_request_setpoint_change_for_state(int state_to
req_mass = (ui->lineEdit_standby_mass ->text()).toFloat();
break;
}
- case PICKER_STATE_LIFT:
+ case PICKER_STATE_LIFT_UP:
{
req_should_smooth = ui->checkbox_lift_up->isChecked();
req_x = (ui->lineEdit_goto_start_x ->text()).toFloat();
@@ -172,7 +266,239 @@ void PickerControllerTab::publish_request_setpoint_change_for_state(int state_to
}
// Publish a ROS message with the setpoint to be requested
+#ifdef CATKIN_MAKE
+ // Initialise the message as a local variable
+ d_fall_pps::SetpointWithHeader msg;
+
+ // Fill the header of the message
+ fillSetpointMessageHeader( msg );
+
+ // Fill in the (x,y,z,yaw,mass) values
+ msg.x = req_x;
+ msg.y = req_y;
+ msg.z = req_z;
+ msg.yaw = req_yaw * DEG2RAD;
+ msg.mass = req_mass;
+
+ // Fill in the "should smooth" value
+ msg.isChecked = req_should_smooth;
+
+ // Fill in the "picker state" value
+ msg.buttonID = state_to_publish;
+
+ // Publish the setpoint
+ this->requestSetpointChangePublisher.publish(msg);
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[PICKER CONTROLLER GUI] Published request for setpoint change to: [" << req_x << ", "<< req_y << ", "<< req_z << ", "<< req_yaw << "], with {mass,smooth,state} = " << req_mass << ", "<< req_should_smooth << ", "<< state_to_publish );
+#else
+ // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
+ QTextStream(stdout) << "[PICKER CONTROLLER GUI] would publish request for: [" << req_x << ", "<< req_y << ", "<< req_z << ", "<< req_yaw << "], with {mass,smooth,state} = " << req_mass << ", "<< req_should_smooth << ", "<< state_to_publish;
+#endif
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// S E T P P O O I NN N T
+// SSS EEE T PPPP O O I N N N T
+// S E T P O O I N NN T
+// SSSS EEEEE T P OOO III N N T
+//
+// CCCC H H A N N GGGG EEEEE DDDD
+// C H H A A NN N G E D D
+// C HHHHH A A N N N G EEE D D
+// C H H AAAAA N NN G G E D D
+// CCCC H H A A N N GGGG EEEEE DDDD
+//
+// 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
+// ----------------------------------------------------------------------------------
+
+
+#ifdef CATKIN_MAKE
+void PickerControllerTab::setpointChangedCallback(const d_fall_pps::SetpointWithHeader& newSetpoint)
+{
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+
+ // EXTRACT THE SETPOINT
+ float x = newSetpoint.x;
+ float y = newSetpoint.y;
+ float z = newSetpoint.z;
+ float yaw = newSetpoint.yaw;
+
+ float mass = newSetpoint.yaw;
+ bool smooth = newSetpoint.isChecked;
+ int state = newSetpoint.buttonID;
+
+ // UPDATE THE SETPOINT LineEdits
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_x->setText(qstr + QString::number( x, 'f', DECIMAL_PLACES_POSITION));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_y->setText(qstr + QString::number( y, 'f', DECIMAL_PLACES_POSITION));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_z->setText(qstr + QString::number( z, 'f', DECIMAL_PLACES_POSITION));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+
+ ui->lineEdit_current_mass->setText(qstr + QString::number( mass, 'f', DECIMAL_PLACES_MASS_GRAMS));
+
+ ui->checkbox_current->setChecked(smooth);
+
+
+ // MAKE THE "GREEN FRAME" VISIBLE ONLY FOR THE CURRENT STATE
+
+ // First make all of them NOT visible
+ ui->frame_goto_start_active->setVisible(false);
+ ui->frame_attach_active ->setVisible(false);
+ ui->frame_lift_up_active ->setVisible(false);
+ ui->frame_goto_end_active ->setVisible(false);
+ ui->frame_put_down_active ->setVisible(false);
+ ui->frame_squat_active ->setVisible(false);
+ ui->frame_jump_active ->setVisible(false);
+ ui->frame_standby_active ->setVisible(false);
+
+ // Switch between the possible states
+ switch (state)
+ {
+ case PICKER_STATE_STANDBY:
+ {
+ ui->frame_standby_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_GOTO_START:
+ {
+ ui->frame_goto_start_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_ATTACH:
+ {
+ ui->frame_attach_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_LIFT_UP:
+ {
+ ui->frame_lift_up_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_GOTO_END:
+ {
+ ui->frame_goto_end_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_PUT_DOWN:
+ {
+ ui->frame_put_down_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_SQUAT:
+ {
+ ui->frame_squat_active->setVisible(true);
+ break;
+ }
+ case PICKER_STATE_JUMP:
+ {
+ ui->frame_standby_active->setVisible(true);
+ break;
+ }
+ default:
+ {
+ break;
+ }
+ }
+
+}
+#endif
+
+
+
+
+// ----------------------------------------------------------------------------------
+// PPPP OOO SSSS EEEEE DDDD A TTTTT A
+// P P O O S E D D A A T A A
+// PPPP O O SSS EEE D D A A T A A
+// P O O S E D D AAAAA T AAAAA
+// P OOO SSSS EEEEE DDDD A A T A A
+// ----------------------------------------------------------------------------------
+
+
+void PickerControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
+{
+ if (!occluded)
+ {
+ // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
+ QString qstr = "";
+ // UPDATE THE MEASUREMENT COLUMN
+ if (x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_x->setText(qstr + QString::number( x, 'f', DECIMAL_PLACES_POSITION));
+ if (y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_y->setText(qstr + QString::number( y, 'f', DECIMAL_PLACES_POSITION));
+ if (z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_z->setText(qstr + QString::number( z, 'f', DECIMAL_PLACES_POSITION));
+
+ if (yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_measured_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+
+ // GET THE CURRENT SETPOINT
+ float error_x = x - (ui->lineEdit_current_x->text() ).toFloat();
+ float error_y = y - (ui->lineEdit_current_y->text() ).toFloat();
+ float error_z = z - (ui->lineEdit_current_z->text() ).toFloat();
+ float error_yaw = yaw - (ui->lineEdit_current_yaw->text()).toFloat();
+
+ // UPDATE THE ERROR COLUMN
+ if (error_x < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', DECIMAL_PLACES_POSITION));
+ if (error_y < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_y->setText(qstr + QString::number( error_y, 'f', DECIMAL_PLACES_POSITION));
+ if (error_z < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', DECIMAL_PLACES_POSITION));
+
+ if (error_yaw < 0.0f) qstr = ""; else qstr = "+";
+ ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw * RAD2DEG, 'f', DECIMAL_PLACES_ANGLE_DEGREES));
+
+ // Ensure the red frames are not visible
+ if ( ui->frame_x_unavailable->isVisible() )
+ {
+ ui->frame_x_unavailable ->setVisible(false);
+ ui->frame_y_unavailable ->setVisible(false);
+ ui->frame_z_unavailable ->setVisible(false);
+ ui->frame_yaw_unavailable->setVisible(false);
+ }
+ }
+ else
+ {
+ // Make visible the red frames to indicate occluded
+ if ( !(ui->frame_x_unavailable->isVisible()) )
+ {
+ ui->frame_x_unavailable ->setVisible(true);
+ ui->frame_y_unavailable ->setVisible(true);
+ ui->frame_z_unavailable ->setVisible(true);
+ ui->frame_yaw_unavailable->setVisible(true);
+ }
+ }
+}
+
+void PickerControllerTab::poseDataUnavailableSlot()
+{
+ ui->lineEdit_measured_x->setText("xx.xx");
+ ui->lineEdit_measured_y->setText("xx.xx");
+ ui->lineEdit_measured_z->setText("xx.xx");
+
+ ui->lineEdit_measured_yaw->setText("xx.xx");
+
+ ui->lineEdit_error_x->setText("xx.xx");
+ ui->lineEdit_error_y->setText("xx.xx");
+ ui->lineEdit_error_z->setText("xx.xx");
+ ui->lineEdit_error_yaw->setText("xx.xx");
}
@@ -200,7 +526,7 @@ void PickerControllerTab::on_button_attach_clicked()
void PickerControllerTab::on_button_lift_up_clicked()
{
// Directly call the function that publishes state (and setpoint) changes
- publish_request_setpoint_change_for_state(PICKER_STATE_LIFT);
+ publish_request_setpoint_change_for_state(PICKER_STATE_LIFT_UP);
}
void PickerControllerTab::on_button_goto_end_clicked()
@@ -271,7 +597,7 @@ void PickerControllerTab::on_checkbox_lift_up_clicked()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -357,7 +683,7 @@ void PickerControllerTab::on_button_goto_start_inc_minus_x_clicked()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -376,7 +702,7 @@ void PickerControllerTab::on_button_goto_start_inc_plus_x_clicked()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -395,7 +721,7 @@ void PickerControllerTab::on_button_goto_start_inc_minus_y_clicked()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -414,7 +740,7 @@ void PickerControllerTab::on_button_goto_start_inc_plus_y_clicked()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -467,7 +793,7 @@ void PickerControllerTab::on_button_goto_start_inc_minus_yaw_clicked()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -486,7 +812,7 @@ void PickerControllerTab::on_button_goto_start_inc_plus_yaw_clicked()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -541,7 +867,7 @@ void PickerControllerTab::on_button_lift_up_inc_minus_z_clicked()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
@@ -559,7 +885,7 @@ void PickerControllerTab::on_button_lift_up_inc_plus_z_clicked()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
@@ -577,7 +903,7 @@ void PickerControllerTab::on_button_lift_up_inc_minus_mass_clicked()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
@@ -596,7 +922,7 @@ void PickerControllerTab::on_button_lift_up_inc_plus_mass_clicked()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
@@ -1036,7 +1362,7 @@ void PickerControllerTab::on_lineEdit_goto_start_x_editingFinished()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -1052,7 +1378,7 @@ void PickerControllerTab::on_lineEdit_goto_start_y_editingFinished()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -1082,7 +1408,7 @@ void PickerControllerTab::on_lineEdit_goto_start_yaw_editingFinished()
QVector<int> states_for_which_this_change_applies;
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_START);
states_for_which_this_change_applies.append(PICKER_STATE_ATTACH);
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
}
@@ -1114,7 +1440,7 @@ void PickerControllerTab::on_lineEdit_lift_up_z_editingFinished()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
}
@@ -1129,7 +1455,7 @@ void PickerControllerTab::on_lineEdit_lift_up_mass_editingFinished()
if (ui->checkbox_should_publish_value_changed->isChecked())
{
QVector<int> states_for_which_this_change_applies;
- states_for_which_this_change_applies.append(PICKER_STATE_LIFT);
+ states_for_which_this_change_applies.append(PICKER_STATE_LIFT_UP);
states_for_which_this_change_applies.append(PICKER_STATE_GOTO_END);
states_for_which_this_change_applies.append(PICKER_STATE_PUT_DOWN);
publish_setpoint_if_current_state_matches(states_for_which_this_change_applies);
@@ -1349,3 +1675,237 @@ void PickerControllerTab::on_lineEdit_increment_mass_editingFinished()
float value_entered = (ui->lineEdit_increment_mass->text()).toFloat();
ui->lineEdit_increment_mass->setText(QString::number( value_entered, 'f', DECIMAL_PLACES_MASS_GRAMS));
}
+
+
+// ----------------------------------------------------------------------------------
+// A GGGG EEEEE N N TTTTT III DDDD SSSS
+// A A G E NN N T I D D S
+// A A G EEE N N N T I D D SSS
+// AAAAA G G E N NN T I D D S
+// A A GGGG EEEEE N N T III DDDD SSSS
+// ----------------------------------------------------------------------------------
+
+
+void PickerControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll)
+{
+
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ m_shouldCoordinateAll = shouldCoordinateAll;
+ // Clear the previous list of agent IDs
+ m_vector_of_agentIDs_toCoordinate.clear();
+ // Copy across the agent IDs, if necessary
+ if (!shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < agentIDs.length() ; irow++ )
+ {
+ m_vector_of_agentIDs_toCoordinate.push_back( agentIDs[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+
+
+#ifdef CATKIN_MAKE
+ // If there is only one agent to coordinate,
+ // then subscribe to the relevant data
+ if (agentIDs.length() == 1)
+ {
+
+ // // > Create the appropriate node handle
+ QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+ ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+ // // > Request the current setpoint
+ ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<d_fall_pps::GetSetpointService>("PickerControllerService/GetCurrentSetpoint", false);
+ d_fall_pps::GetSetpointService getSetpointCall;
+ getSetpointCall.request.data = 0;
+ getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+ if(getCurrentSetpointServiceClient.call(getSetpointCall))
+ {
+ setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+ }
+ else
+ {
+ // Inform the user
+ ROS_INFO("[PICKER CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+ }
+
+ // SUBSCRIBERS
+ // > For receiving message that the setpoint was changed
+ setpointChangedSubscriber = agent_base_nodeHandle.subscribe("PickerControllerService/SetpointChanged", 1, &PickerControllerTab::setpointChangedCallback, this);
+ }
+ else
+ {
+ // Unsubscribe
+ setpointChangedSubscriber.shutdown();
+
+ // Set information back to the default
+ ui->lineEdit_current_x ->setText("xx.xx");
+ ui->lineEdit_current_y ->setText("xx.xx");
+ ui->lineEdit_current_z ->setText("xx.xx");
+ ui->lineEdit_current_yaw ->setText("xx.xx");
+ ui->lineEdit_current_mass->setText("xx.xx");
+
+ }
+#endif
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M SSSS GGG H H EEEEE A DDDD EEEEE RRRR
+// MM MM S G G H H E A A D D E R R
+// M M M SSS G HHHHH EEE A A D D EEE RRRR
+// M M S G G H H E AAAAA D D E R R
+// M M SSSS GGGG H H EEEEE A A DDDD EEEEE R R
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+// Fill the header for a message
+void PickerControllerTab::fillSetpointMessageHeader( d_fall_pps::SetpointWithHeader & msg )
+{
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ msg.shouldCheckForAgentID = false;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ // Lock the mutex
+ m_agentIDs_toCoordinate_mutex.lock();
+ // Add the "coordinate all" flag
+ msg.shouldCheckForAgentID = !(m_shouldCoordinateAll);
+ // Add the agent IDs if necessary
+ if (!m_shouldCoordinateAll)
+ {
+ for ( int irow = 0 ; irow < m_vector_of_agentIDs_toCoordinate.size() ; irow++ )
+ {
+ msg.agentIDs.push_back( m_vector_of_agentIDs_toCoordinate[irow] );
+ }
+ }
+ // Unlock the mutex
+ m_agentIDs_toCoordinate_mutex.unlock();
+ break;
+ }
+
+ default:
+ {
+ msg.shouldCheckForAgentID = true;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+}
+#endif
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III DDDD &&& TTTTT Y Y PPPP EEEEE
+// I D D & T Y Y P P E
+// I D D & T Y PPPP EEE
+// I D D & & & T Y P E
+// III DDDD &&& T Y P EEEEE
+// ----------------------------------------------------------------------------------
+
+
+
+#ifdef CATKIN_MAKE
+bool PickerControllerTab::getTypeAndIDParameters()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // 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("~");
+
+ // Get the value of the "type" parameter into a local string variable
+ std::string type_string;
+ if(!nodeHandle.getParam("type", type_string))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Failed to get type");
+ }
+
+ // Set the "m_type" class variable based on this string loaded
+ if ((!type_string.compare("coordinator")))
+ {
+ m_type = TYPE_COORDINATOR;
+ }
+ else if ((!type_string.compare("agent")))
+ {
+ m_type = TYPE_AGENT;
+ }
+ else
+ {
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] The 'type' parameter retrieved was not recognised.");
+ }
+
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PICKER CONTROLLER TAB GUI] Is of type AGENT with ID = " << m_ID);
+ }
+ break;
+ }
+
+ // A COORDINATOR TYPE PARAMETER SERVICE IS REQUESTED FROM:
+ // > The master GUI
+ case TYPE_COORDINATOR:
+ {
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PICKER CONTROLLER TAB GUI] Is of type COORDINATOR with ID = " << m_ID);
+ }
+ break;
+ }
+
+ default:
+ {
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PICKER CONTROLLER TAB GUI] The 'm_type' variable was not recognised.");
+ break;
+ }
+ }
+
+ // Return
+ return return_was_successful;
+}
+#endif
diff --git a/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg b/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg
index ff412908b37d14516ae13d70209dd218c95b2277..cdf1562a19264e182d534bd33d4fc12a2413c5fe 100644
--- a/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg
+++ b/pps_ws/src/d_fall_pps/msg/SetpointWithHeader.msg
@@ -2,6 +2,7 @@ float64 x
float64 y
float64 z
float64 yaw
+float64 mass
bool isChecked
uint32 buttonID
bool shouldCheckForAgentID