ViconDataPublisher.cpp 6.91 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

bucyril's avatar
bucyril committed
17
18
#include <string.h>
#include "DataStreamClient.h"
Philipp Friedli's avatar
Philipp Friedli committed
19
#include "ros/ros.h"
20
#include "d_fall_pps/ViconData.h"
roangel's avatar
roangel committed
21
#include "d_fall_pps/UnlabeledMarker.h"
Philipp Friedli's avatar
Philipp Friedli committed
22

muelmarc's avatar
merging    
muelmarc committed
23
#define TESTING_FAKE_DATA
24
25

// notice that unit here are in milimeters
bucyril's avatar
bucyril committed
26
using namespace ViconDataStreamSDK::CPP;
27
using namespace d_fall_pps;
bucyril's avatar
bucyril committed
28

29
int main(int argc, char* argv[]) {
bucyril's avatar
bucyril committed
30
    ros::init(argc, argv, "ViconDataPublisher");
31
32
33
34
35

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

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

38
39
    #ifdef TESTING_FAKE_DATA
    // Test faking data part
40
41
    float f = 0;
    int i = 0;
42
43

    while(ros::ok())
44
    {
45
46
47
48
        if(i % 1000 == 0)
        {
        	ROS_INFO("iteration #%d",i);
    	}
roangel's avatar
roangel committed
49
50

        // Testing piece of code
muelmarc's avatar
merging    
muelmarc committed
51
        ViconData viconData;
roangel's avatar
roangel committed
52
53
54
55
56
57
58
        UnlabeledMarker marker;

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

muelmarc's avatar
merging    
muelmarc committed
59
60
        viconData.markers.push_back(marker);

roangel's avatar
roangel committed
61
62
63
64
65
66

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

muelmarc's avatar
merging    
muelmarc committed
67
        viconData.markers.push_back(marker);
68
69
70
71
72
73
74

        if(i > 50 && i < 100)
        {
            marker.index = 2;
            marker.x = f;
            marker.y = f;
            marker.z = 0;
muelmarc's avatar
merging    
muelmarc committed
75
            viconData.markers.push_back(marker);
76
        }
77

78
        ros::Duration(0.1).sleep();
79
        f += 10;
80
        i++;
muelmarc's avatar
merging    
muelmarc committed
81
82
83
        // TODO: Fake CF data

        viconDataPublisher.publish(viconData); // testing data
84
85
    }
    #else
86
87


88
89
    Client client;

90
    std::string hostName;
bucyril's avatar
bucyril committed
91
92
93
94
95
    if(!nodeHandle.getParam("hostName", hostName)) {
        ROS_ERROR("Failed to get hostName");
        return 1;
    }

96
97
98
99
100
101
    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
102
            ros::Duration(1.0).sleep();
103
        } else {
bucyril's avatar
bucyril committed
104
105
106
107
            ROS_INFO("Connected successfully");
        }
    }

108
    //set data stream parameters
bucyril's avatar
bucyril committed
109
    client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ServerPush); //maybe ServerPush instead of ClientPull for less latency
bucyril's avatar
bucyril committed
110

111
112
113
114
    client.EnableSegmentData();
    client.EnableMarkerData();
    client.EnableUnlabeledMarkerData();
    client.EnableDeviceData();
bucyril's avatar
bucyril committed
115

116
    // Set the global up axis, such that Z is up
117
    client.SetAxisMapping(Direction::Forward, Direction::Left, Direction::Up);
bucyril's avatar
bucyril committed
118

119
120
121
122
123
124
    while (ros::ok()) {
        // 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
125

126
127
        ViconData viconData;

roangel's avatar
roangel committed
128
129
130
131
        // Unlabeled markers, for GUI
        unsigned int unlabeledMarkerCount = client.GetUnlabeledMarkerCount().MarkerCount;

        UnlabeledMarker marker;
132
        ROS_INFO_STREAM("unlabeledMarkerCount: " << unlabeledMarkerCount);
roangel's avatar
roangel committed
133

134
        for(int unlabeledMarkerIndex = 0; unlabeledMarkerIndex < unlabeledMarkerCount; unlabeledMarkerIndex++)
roangel's avatar
roangel committed
135
136
137
138
139
140
141
142
143
144
        {

            Output_GetUnlabeledMarkerGlobalTranslation OutputTranslation =
                client.GetUnlabeledMarkerGlobalTranslation(unlabeledMarkerIndex);

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

145
            viconData.markers.push_back(marker);
roangel's avatar
roangel committed
146
147
        }

148
149
150
151
        unsigned int subjectCount = client.GetSubjectCount().SubjectCount;

        // //Process the data and publish on topic
        for (int index = 0; index < subjectCount; index++) {
152
       		std::string subjectName = client.GetSubjectName(index).SubjectName;
bucyril's avatar
bucyril committed
153
            std::string segmentName = client.GetSegmentName(subjectName, 0).SegmentName;
154

155

156
            //continue only if the received frame is for the correct crazyflie
157
158
            Output_GetSegmentGlobalTranslation outputTranslation =
                    client.GetSegmentGlobalTranslation(subjectName, segmentName);
159
            //ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
160
161
162

            Output_GetSegmentGlobalRotationQuaternion outputRotation =
                    client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
163
            //ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182

            //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
183
184
            }

185
            //build message
186
187
188
189
190
191
192
193
194
195
196
197
            CrazyflieData cfData;
            cfData.crazyflieName = subjectName;

            cfData.x = outputTranslation.Translation[0] / 1000.0f;
            cfData.y = outputTranslation.Translation[1] / 1000.0f;
            cfData.z = outputTranslation.Translation[2] / 1000.0f;
            cfData.roll = roll;
            cfData.pitch = pitch;
            cfData.yaw = yaw;
            cfData.acquiringTime = totalViconLatency;

            viconData.crazyflies.push_back(cfData);
bucyril's avatar
bucyril committed
198
        }
199
        viconDataPublisher.publish(viconData);
200
    }
bucyril's avatar
bucyril committed
201

202
203
204
205
    client.DisableSegmentData();
    client.DisableMarkerData();
    client.DisableUnlabeledMarkerData();
    client.DisableDeviceData();
bucyril's avatar
bucyril committed
206

207
    client.Disconnect();
208
209

    #endif
Philipp Friedli's avatar
Philipp Friedli committed
210
}