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

bucyril's avatar
bucyril committed
22
23
using namespace ViconDataStreamSDK::CPP;

24
int main(int argc, char* argv[]) {
bucyril's avatar
bucyril committed
25
    ros::init(argc, argv, "ViconDataPublisher");
26
27
28
29
30
31
32

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

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

33
34
35
36
37
38
39
40
41
42
43
44
45
    //publish something random if no viconData is available for testing
    /*
    d_fall_pps::ViconData viconData; 
    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
    */

46
47
48
    Client client;

    //connect client to Vicon computer
bucyril's avatar
bucyril committed
49
    std::string hostName = "10.42.00.15:801";
50
51
52
53
54
55
    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
56
            ros::Duration(1.0).sleep();
57
        } else {
bucyril's avatar
bucyril committed
58
59
60
61
            ROS_INFO("Connected successfully");
        }
    }

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

65
66
67
68
    client.EnableSegmentData();
    client.EnableMarkerData();
    client.EnableUnlabeledMarkerData();
    client.EnableDeviceData();
bucyril's avatar
bucyril committed
69

70
    // Set the global up axis, such that Z is up
71
72
73
    client.SetAxisMapping(Direction::Forward,
            Direction::Left,
            Direction::Up);
bucyril's avatar
bucyril committed
74

75
76
77
78
    //TODO
    //maybe we need a loop rate?---------e.g. 0.5 MHz -----------------------------
    //ros::Rate loop_rate(500000)

79
80
81
82
83
84
    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
85

86
87
88
89
        unsigned int subjectCount = client.GetSubjectCount().SubjectCount;

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

93
94
95
96
97
98
99

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

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

100

101
102
103
104
105
            //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];
106
107
108
            ROS_INFO("x-value and w-value:");
            ROS_INFO_STREAM(quat_x);
            ROS_INFO_STREAM(quat_w);
109
110
111
112
113

            //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));
114
115
116
            ROS_INFO("roll:");
            ROS_INFO_STREAM(roll);
            ROS_INFO("----------------------");
117
118
119
120
121
122
123
124

            //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
125
126
            }

127
128
129
            //build and publish message
            d_fall_pps::ViconData viconData;
            viconData.crazyflieName = subjectName;
130

131
132
133
134
135
136
137
138
            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;

139
            //finally publish
140
            viconDataPublisher.publish(viconData);
141

bucyril's avatar
bucyril committed
142
143
        }

144
    }
bucyril's avatar
bucyril committed
145

146
147
148
149
    client.DisableSegmentData();
    client.DisableMarkerData();
    client.DisableUnlabeledMarkerData();
    client.DisableDeviceData();
bucyril's avatar
bucyril committed
150

151
    client.Disconnect();
Philipp Friedli's avatar
Philipp Friedli committed
152
}