ViconDataPublisher.cpp 6.11 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"
Philipp Friedli's avatar
Philipp Friedli committed
33

bucyril's avatar
bucyril committed
34
using namespace ViconDataStreamSDK::CPP;
35
using namespace d_fall_pps;
bucyril's avatar
bucyril committed
36

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

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

    ros::Publisher viconDataPublisher =
44
            nodeHandle.advertise<ViconData>("ViconData", 1);
45

46
47
    //publish something random if no viconData is available for testing
    /*
48
    ViconData viconData; 
49
50
51
52
53
54
55
56
57
58
    double i = 1; 
    while(true)
    {
    	viconData.roll  = i;
    	 viconDataPublisher.publish(viconData);
    	 ++i;
    }
    //the code will not go further than here if testing without real ViconData
    */

59
60
61
    Client client;

    //connect client to Vicon computer
bucyril's avatar
bucyril committed
62
    std::string hostName = "10.42.00.15:801";
63
64
65
66
67
68
    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
69
            ros::Duration(1.0).sleep();
70
        } else {
bucyril's avatar
bucyril committed
71
72
73
74
            ROS_INFO("Connected successfully");
        }
    }

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

78
79
80
81
    client.EnableSegmentData();
    client.EnableMarkerData();
    client.EnableUnlabeledMarkerData();
    client.EnableDeviceData();
bucyril's avatar
bucyril committed
82

83
    // Set the global up axis, such that Z is up
84
85
86
    client.SetAxisMapping(Direction::Forward,
            Direction::Left,
            Direction::Up);
bucyril's avatar
bucyril committed
87

88
89
90
91
    //TODO
    //maybe we need a loop rate?---------e.g. 0.5 MHz -----------------------------
    //ros::Rate loop_rate(500000)

92
93

    int iterations = 0;
94
    while (ros::ok()) {
95
96
97
98
99
100
101
102
103
104
    	//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++;




105
106
107
108
109
        // 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
110

111
112
113
114
        unsigned int subjectCount = client.GetSubjectCount().SubjectCount;

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

118
119
120

            Output_GetSegmentGlobalTranslation outputTranslation =
                    client.GetSegmentGlobalTranslation(subjectName, segmentName);
121
            ROS_INFO_STREAM("translation occluded: " << outputTranslation.Occluded);
122
123
124

            Output_GetSegmentGlobalRotationQuaternion outputRotation =
                    client.GetSegmentGlobalRotationQuaternion(subjectName, segmentName);
125
            ROS_INFO_STREAM("translation occluded: " << outputRotation.Occluded);
126

127

128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
            //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
146
147
            }

148
            //build and publish message
149
            ViconData viconData;
150
            viconData.crazyflieName = subjectName;
151

phfriedl's avatar
phfriedl committed
152
153
154
            viconData.x = outputTranslation.Translation[0] / 1000.0f;
            viconData.y = outputTranslation.Translation[1] / 1000.0f;
            viconData.z = outputTranslation.Translation[2] / 1000.0f;
155
156
157
158
159
            viconData.roll = roll;
            viconData.pitch = pitch;
            viconData.yaw = yaw;
            viconData.acquiringTime = totalViconLatency;

160
            //finally publish
161
            viconDataPublisher.publish(viconData);
162

bucyril's avatar
bucyril committed
163
164
        }

165
    }
bucyril's avatar
bucyril committed
166

167
168
169
170
    client.DisableSegmentData();
    client.DisableMarkerData();
    client.DisableUnlabeledMarkerData();
    client.DisableDeviceData();
bucyril's avatar
bucyril committed
171

172
    client.Disconnect();
Philipp Friedli's avatar
Philipp Friedli committed
173
}