UWBManagerService.cpp 3.88 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//    ROS node that provides the anchor positions from the yaml file
//    Copyright (C) 2017  Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include <string>
#include <vector>

Yvan Bosshard's avatar
Yvan Bosshard committed
20
21
22
23
#include <ros/package.h>

#include "d_fall_pps/Anchors.h"

24
#include "UWBManagerService.h"
Yvan Bosshard's avatar
Yvan Bosshard committed
25

26
27
#include "DataListener.h"

28
29
30
31
#define UWB_UPDATE_DISABLE      0
#define UWB_UPDATE_ENABLE       1
#define UWB_UPDATE_ANCHORS      5
#define UWB_CALIBRATE_ANCHORS   7
michaero's avatar
michaero committed
32

33
34
using namespace d_fall_pps;

michaero's avatar
michaero committed
35
bool enableUWB;
36
bool calSuccess = false;
37
UWBAnchorArray anchors;
38
DataListener selfLoc;
39
40
41

int main(int argc, char* argv[])
{
42
    ros::init(argc, argv, "UWBManagerService");
43
    ros::NodeHandle nodeHandle("~");
44
45
46
    
    readYaml();

47
    ros::ServiceServer uwbService = nodeHandle.advertiseService("UWBData", getAnchorPositions);
48

michaero's avatar
michaero committed
49
    ros::Subscriber uwbUpdate = nodeHandle.subscribe("/my_GUI/UWBUpdate", 1, updateCallback);
50
51

    ros::spin();
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66

    return 0;
}

void d_fall_pps::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 d_fall_pps::readYaml()
{
67
    ros::NodeHandle nodeHandle("~");
68
69
70
71
72
73
74
75
    int length;
    if(!nodeHandle.getParam("anchorLength", length))
    {
        ROS_ERROR("Failed to get Anchors");
        return;
    }

    anchors.length = length;
76
    anchors.data.clear();
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
    
    std::vector<float> anchorArr;
    UWBAnchor tempAnchor;

    for(int i = 0; i < length; i++)
    {
        loadParameterFloatVector(nodeHandle, "anchor" + std::to_string(i), anchorArr, 3);

        tempAnchor.id = i;
        tempAnchor.x = anchorArr[0];
        tempAnchor.y = anchorArr[1];
        tempAnchor.z = anchorArr[2];

        anchors.data.push_back(tempAnchor);
    }

    return;
}

96
bool d_fall_pps::getAnchorPositions(Anchors::Request& request, Anchors::Response& response)
97
{
98
99
100
101
102
103
104
    selfLoc.setOffset(request.x, request.y, request.z);

    if(request.invert != 0)
    {
        for(int i = 0; i < 6; ++i)
        {
            if(request.invert & (1 << i))
105
                selfLoc.invertAnchor(i);
106
107
108
        }
    }

109
    readYaml();
tiagos's avatar
tiagos committed
110

michaero's avatar
michaero committed
111
    response.enableUWB = enableUWB;
112
    response.anchorArray = anchors;
113
    response.calSuccess = calSuccess;
114

115
116
117
    return true;
}

michaero's avatar
michaero committed
118
void d_fall_pps::updateCallback(const std_msgs::Int32 &message)
119
{
michaero's avatar
michaero committed
120
121
122
123
124
    switch(message.data)
    {
    case UWB_UPDATE_DISABLE:
        enableUWB = false;
        break;
125
126
    case UWB_CALIBRATE_ANCHORS:
        calibrateAnchors();
tiagos's avatar
tiagos committed
127
        readYaml();
tiagos's avatar
tiagos committed
128
        break;
michaero's avatar
michaero committed
129
130
131
132
133
134
    case UWB_UPDATE_ENABLE:
        enableUWB = true;
    case UWB_UPDATE_ANCHORS:
        readYaml();
        break;
    default:
135
        ROS_WARN("[UWBManagerService] unknown update command");
michaero's avatar
michaero committed
136
    }
Yvan Bosshard's avatar
Yvan Bosshard committed
137
    
138
    ROS_WARN("[UWBManagerService] Updated UWB Settings!");
139
}
140
141
142

void d_fall_pps::calibrateAnchors() 
{
tiagos's avatar
tiagos committed
143
    ROS_WARN("[UWBManagerService] Calibrating Anchors...");
tiagos's avatar
tiagos committed
144

tiagos's avatar
tiagos committed
145
    calSuccess = selfLoc.startAnchorLocalistaion();
146
147

    if(calSuccess)
tiagos's avatar
tiagos committed
148
149
150
        ROS_WARN("[UWBManagerService] Calibration successfull!");
    else
        ROS_ERROR("[UWBManagerService] Calibration failed!");
151
}