ViconDataPublisher.cpp 7.41 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
78

        // 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);
        unlabeledMarkersPublisher.publish(markersArray);
79
80
81
        ros::Duration(0.1).sleep();
        f += 0.01;
        i++;
82
    }
roangel's avatar
roangel committed
83
84
85


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

87
88
89
    Client client;

    //connect client to Vicon computer
bucyril's avatar
bucyril committed
90
    std::string hostName = "10.42.00.15:801";
91
92
93
94
95
96
    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
97
            ros::Duration(1.0).sleep();
98
        } else {
bucyril's avatar
bucyril committed
99
100
101
102
            ROS_INFO("Connected successfully");
        }
    }

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

106
107
108
109
    client.EnableSegmentData();
    client.EnableMarkerData();
    client.EnableUnlabeledMarkerData();
    client.EnableDeviceData();
bucyril's avatar
bucyril committed
110

111
    // Set the global up axis, such that Z is up
112
113
114
    client.SetAxisMapping(Direction::Forward,
            Direction::Left,
            Direction::Up);
bucyril's avatar
bucyril committed
115

116
117
118
119
    //TODO
    //maybe we need a loop rate?---------e.g. 0.5 MHz -----------------------------
    //ros::Rate loop_rate(500000)

120
121

    int iterations = 0;
122
    while (ros::ok()) {
123
124
125
126
127
128
129
130
131
    	//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++;



132
        // Output_GetUnlabeledMarkerCount GetUnlabeledMarkerCount() const;
133
134
135
136
137
        // 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
138

roangel's avatar
roangel committed
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
        // 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);

162
163
164
165
        unsigned int subjectCount = client.GetSubjectCount().SubjectCount;

        // //Process the data and publish on topic
        for (int index = 0; index < subjectCount; index++) {
166
167
168
       		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

169
170
171
172
173
174
175

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

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

176

177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
            //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
195
196
            }

197
            //build and publish message
198
            ViconData viconData;
199
            viconData.crazyflieName = subjectName;
200

201
202
203
204
205
206
207
208
            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;

209
            //finally publish
210
            viconDataPublisher.publish(viconData);
211

bucyril's avatar
bucyril committed
212
213
        }

214
    }
bucyril's avatar
bucyril committed
215

216
217
218
219
    client.DisableSegmentData();
    client.DisableMarkerData();
    client.DisableUnlabeledMarkerData();
    client.DisableDeviceData();
bucyril's avatar
bucyril committed
220

221
    client.Disconnect();
Philipp Friedli's avatar
Philipp Friedli committed
222
}