Commit f23b2290 authored by tiagos's avatar tiagos
Browse files

Updated UWB Service and renamed it to follow standard

parent 6d5233e4
......@@ -224,7 +224,7 @@ void MainGUIWindow::_init()
ros::NodeHandle nodeHandle("~");
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBServiceManager/UWBData", false);
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
......@@ -979,7 +979,8 @@ void MainGUIWindow::on_load_from_DB_button_clicked()
void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
{
// Send Command to enable UWB to UWBServiceClient
// Send Command to enable UWB to UWBManagerService
std_msgs::Int32 server_msg;
if(checked)
......@@ -1007,22 +1008,18 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
strm << a.response.anchorArray.data[i].id << "\t" << a.response.anchorArray.data[i].x << "\t"
<< a.response.anchorArray.data[i].y << "\t" << a.response.anchorArray.data[i].z << "\n";
//ROS_ERROR("Anchor %i, %f, %f, %f", , , , );
}
ui->list_anchors->addItem(QString::fromStdString(strm.str()));
}
else
{
<<<<<<< HEAD
ui->checkBox_enable_UWB->setEnabled(!checked);
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
=======
ui->checkBox_enable_UWB->setChecked(!checked);
>>>>>>> 5eaa27f8392291efd232e6a30dc55cd29652d395
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
return;
}
// Publish message that clients have to reload data from UWBServiceClient
// Publish message that clients have to reload data from UWBManagerService
std_msgs::Int32 msg;
msg.data = 1;
UWBSettings_publisher.publish(msg);
......@@ -1030,8 +1027,38 @@ void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
void MainGUIWindow::on_reloadAnchors_button_pressed()
{
// Load Data from UWBServiceClient
ui->list_anchors->addItem(QString::fromStdString("Hello Michael"));//clear();
// Load Data from UWBManagerService
std_msgs::Int32 server_msg;
server_msg.data = UWB_UPDATE_ANCHORS;
UWBServiceClientUpdate_publisher.publish(server_msg);
// Receive the updated information
Anchors a;
if(UWBServiceClient.call(a))
{
ui->checkBox_enable_UWB->setChecked(a.response.enableUWB);
// print anchor positions in list_anchors
ui->list_anchors->clear();
std::ostringstream strm;
strm << "id\tx\ty\tz\n";
for(int i = 0; i < a.response.anchorArray.length; ++i)
{
strm << a.response.anchorArray.data[i].id << "\t" << a.response.anchorArray.data[i].x << "\t"
<< a.response.anchorArray.data[i].y << "\t" << a.response.anchorArray.data[i].z << "\n";
}
ui->list_anchors->addItem(QString::fromStdString(strm.str()));
}
else
{
ROS_ERROR("[Teacher GUI] Could not update UWB Settings!");
return;
}
// Publish message that clients have to reload data from UWBServiceClient
std_msgs::Int32 msg;
......
......@@ -14,6 +14,9 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef UWBMANAGERSERVICE_H
#define UWBMANAGERSERVICE_H
#include "ros/ros.h"
#include "d_fall_pps/UWBAnchor.h"
......@@ -35,4 +38,6 @@ namespace d_fall_pps
// Callback to update anchor Positions
void updateCallback(const std_msgs::Int32 &message);
}
\ No newline at end of file
}
#endif // UWBMANAGERSERVICE_H included
......@@ -2,7 +2,7 @@
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<node pkg="d_fall_pps" name="UWBServiceClient" output="screen" type="UWBServiceClient">
<node pkg="d_fall_pps" name="UWBManagerService" output="screen" type="UWBManagerService">
<rosparam command="load" file="$(find d_fall_pps)/param/AnchorPos.yaml" />
</node>
......
anchorLength: 3 # Update this number after adding/removing anchors!!!
anchorLength: 6 # Update this number after adding/removing anchors!!!
# Structure: anchorID [x, y, z] (ID is always from 0 to length-1)
anchor0: [0.0, 0.0, -1.25]
anchor1: [0.0, 1.0, 0.0]
anchor2: [1.0, 0.0, 0.0]
anchor0: [-1.2, -14.0, 5.8]
anchor1: [14.9, -14.0, 15.9]
anchor2: [14.7, 5.8, 5.8]
anchor3: [1.1, 14.6, 15.9]
anchor4: [-13.5, 7.6, 5.8]
anchor5: [-13.4, -8.4, 5.8]
......@@ -21,7 +21,7 @@
#include "d_fall_pps/Anchors.h"
#include "AnchorPositions.h"
#include "UWBManagerService.h"
#define UWB_UPDATE_DISABLE 0
#define UWB_UPDATE_ENABLE 1
......@@ -34,7 +34,7 @@ UWBAnchorArray anchors;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBServiceManager");
ros::init(argc, argv, "UWBManagerService");
ros::NodeHandle nodeHandle("~");
readYaml();
......@@ -111,8 +111,8 @@ void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
readYaml();
break;
default:
ROS_WARN("[UWBServiceManager] unknown update command");
ROS_WARN("[UWBManagerService] unknown update command");
}
ROS_WARN("[UWBServiceManager] Updated UWB Settings!");
ROS_WARN("[UWBManagerService] Updated UWB Settings!");
}
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