ViconDataPublisher.cpp 7.62 KB
Newer Older
bucyril's avatar
bucyril committed
1
2
//    ROS node that publishes the data from the Vicon system
//    Copyright (C) 2017  Dusan Zikovic, Cyrill Burgener, Marco Mueller, Philipp Friedli
Philipp Friedli's avatar
Philipp Friedli committed
3
//
bucyril's avatar
bucyril committed
4
5
6
7
//    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.
Philipp Friedli's avatar
Philipp Friedli committed
8
//
bucyril's avatar
bucyril committed
9
10
11
12
13
14
15
//    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/>.
Philipp Friedli's avatar
Philipp Friedli committed
16

phfriedl's avatar
phfriedl committed
17
18
19
20
21
22
23
24
25
26
27
28



//TODO:
//CentralManager: extract data about room from vicon data
//CentralManager: assign area for each group and those coordinates to PPSClients
//ViconDataPublisher: extract data about room from vicon data in and send also to PPSClient
//PPSClient: Compare data received from CentralManager and ViconDataPublisher and determine in which area you are
//PPSClient: Choose correct controller accoring to current area



bucyril's avatar
bucyril committed
29
30
#include <string.h>
#include "DataStreamClient.h"
Philipp Friedli's avatar
Philipp Friedli committed
31
#include "ros/ros.h"
32
#include "d_fall_pps/ViconData.h"
roangel's avatar
roangel committed
33
34
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
Philipp Friedli's avatar
Philipp Friedli committed
35

bucyril's avatar
bucyril committed
36
using namespace ViconDataStreamSDK::CPP;
37
using namespace d_fall_pps;
bucyril's avatar
bucyril committed
38

39
int main(int argc, char* argv[]) {
bucyril's avatar
bucyril committed
40
    ros::init(argc, argv, "ViconDataPublisher");
41
42
43
44
45

    ros::NodeHandle nodeHandle("~");
    ros::Time::init();

    ros::Publisher viconDataPublisher =
roangel's avatar
roangel committed
46
        nodeHandle.advertise<ViconData>("ViconData", 1);
47

roangel's avatar
roangel committed
48
49
    ros::Publisher unlabeledMarkersPublisher =
        nodeHandle.advertise<UnlabeledMarkersArray>("UnlabeledMarkersArray", 1);
50

51
52
    float f = 0;
    int i = 0;
roangel's avatar
roangel committed
53
54
    // while(ros::ok())
    while(true)
55
    {
56
57
58
59
        if(i % 1000 == 0)
        {
        	ROS_INFO("iteration #%d",i);
    	}
roangel's avatar
roangel committed
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77

        // Testing piece of code
        UnlabeledMarker marker;
        UnlabeledMarkersArray markersArray;

        marker.index = 0;
        marker.x = f;
        marker.y = 0;
        marker.z = 0;

        markersArray.markers.push_back(marker);

        marker.index = 1;
        marker.x = 0;
        marker.y = f;
        marker.z = 0;

        markersArray.markers.push_back(marker);
78
79
80
81
82
83
84
85
86
87

        if(i > 50 && i < 100)
        {
            marker.index = 2;
            marker.x = f;
            marker.y = f;
            marker.z = 0;
            markersArray.markers.push_back(marker);
        }

roangel's avatar
roangel committed
88
        unlabeledMarkersPublisher.publish(markersArray);
89
90
91
        ros::Duration(0.1).sleep();
        f += 0.01;
        i++;
92
    }
roangel's avatar
roangel committed
93
94
95


    //the testing code will not go further than here if testing without real ViconData
96

97
98
99
    Client client;

    //connect client to Vicon computer
bucyril's avatar
bucyril committed
100
    std::string hostName = "10.42.00.15:801";
101
102
103
104
105
106
    ROS_INFO_STREAM("Connecting to " << hostName << " ...");
    while (!client.IsConnected().Connected) {
        bool ok = (client.Connect(hostName).Result == Result::Success);

        if (!ok) {
            ROS_ERROR("Error - connection failed...");
bucyril's avatar
bucyril committed
107
            ros::Duration(1.0).sleep();
108
        } else {
bucyril's avatar
bucyril committed
109
110
111
112
            ROS_INFO("Connected successfully");
        }
    }

113
    //set data stream parameters
114
    client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull); //phfriedl: maybe ServerPush instead of ClientPull for less latency?
bucyril's avatar
bucyril committed
115

116
117
118
119
    client.EnableSegmentData();
    client.EnableMarkerData();
    client.EnableUnlabeledMarkerData();
    client.EnableDeviceData();
bucyril's avatar
bucyril committed
120

121
    // Set the global up axis, such that Z is up
122
123
124
    client.SetAxisMapping(Direction::Forward,
            Direction::Left,
            Direction::Up);
bucyril's avatar
bucyril committed
125

126
127
128
129
    //TODO
    //maybe we need a loop rate?---------e.g. 0.5 MHz -----------------------------
    //ros::Rate loop_rate(500000)

130
131

    int iterations = 0;
132
    while (ros::ok()) {
133
134
135
136
137
138
139
140
141
    	//if you want to see at least some output in the terminal
    	//to see that you are still publishing
    	if(iterations % 1000 == 0){
        	ROS_INFO("iteration #%d",iterations);
    	}
    	iterations++;



142
        // Output_GetUnlabeledMarkerCount GetUnlabeledMarkerCount() const;
143
144
145
146
147
        // Get a frame
        while (client.GetFrame().Result != Result::Success) {
            // Sleep a little so that we don't lumber the CPU with a busy poll
            ros::Duration(0.001).sleep();
        }
bucyril's avatar
bucyril committed
148

roangel's avatar
roangel committed
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
        // Unlabeled markers, for GUI
        unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;

        UnlabeledMarker marker;
        UnlabeledMarkersArray markersArray;


        for(int unlabeledMarkerIndex = 0; i < unlabeledMarkerCount; i++)
        {

            Output_GetUnlabeledMarkerGlobalTranslation OutputTranslation =
                client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);

            marker.index = unlabeledMarkerIndex;
            marker.x = OutputTranslation.Translation[0];
            marker.y = OutputTranslation.Translation[1];
            marker.z = OutputTranslation.Translation[2];

            markersArray.markers.push_back(marker);
        }

        unlabeledMarkersPublisher.publish(markersArray);

172
173
174
175
        unsigned int subjectCount = client.GetSubjectCount().SubjectCount;

        // //Process the data and publish on topic
        for (int index = 0; index < subjectCount; index++) {
176
177
178
       		std::string subjectName = client.GetSubjectName(index).SubjectName;
            std::string segmentName = client.GetSegmentName(subjectName, 0).SegmentName; //last value had to be changed to 0 instead of index, 27.03.17

179
180
181
182
183
184
185

            Output_GetSegmentGlobalTranslation outputTranslation =
                    client.GetSegmentGlobalTranslation(subjectName, segmentName);

            Output_GetSegmentGlobalRotationQuaternion outputRotation =
                    client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);

186

187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
            //calculate position and rotation of Crazyflie
            double quat_x = outputRotation.Rotation[0];
            double quat_y = outputRotation.Rotation[1];
            double quat_z = outputRotation.Rotation[2];
            double quat_w = outputRotation.Rotation[3];

            //TODO check whether this transformation is correct
            double roll = atan2(2 * (quat_w * quat_x + quat_y * quat_z), 1 - 2 * (quat_x * quat_x + quat_y * quat_y));
            double pitch = asin(2 * (quat_w * quat_y - quat_z * quat_x));
            double yaw = atan2(2 * (quat_w * quat_z + quat_x * quat_y), 1 - 2 * (quat_y * quat_y + quat_z * quat_z));

            //calculate time until frame data was received
            Output_GetLatencyTotal outputLatencyTotal = client.GetLatencyTotal();
            double totalViconLatency;
            if (outputLatencyTotal.Result == Result::Success) {
                totalViconLatency = outputLatencyTotal.Total;
            } else {
                totalViconLatency = 0;
bucyril's avatar
bucyril committed
205
206
            }

207
            //build and publish message
208
            ViconData viconData;
209
            viconData.crazyflieName = subjectName;
210

211
212
213
214
215
216
217
218
            viconData.x = outputTranslation.Translation[0];
            viconData.y = outputTranslation.Translation[1];
            viconData.z = outputTranslation.Translation[2];
            viconData.roll = roll;
            viconData.pitch = pitch;
            viconData.yaw = yaw;
            viconData.acquiringTime = totalViconLatency;

219
            //finally publish
220
            viconDataPublisher.publish(viconData);
221

bucyril's avatar
bucyril committed
222
223
        }

224
    }
bucyril's avatar
bucyril committed
225

226
227
228
229
    client.DisableSegmentData();
    client.DisableMarkerData();
    client.DisableUnlabeledMarkerData();
    client.DisableDeviceData();
bucyril's avatar
bucyril committed
230

231
    client.Disconnect();
Philipp Friedli's avatar
Philipp Friedli committed
232
}