ViconDataPublisher.cpp 4.65 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
33
34
35

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

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

    Client client;

    //connect client to Vicon computer
bucyril's avatar
bucyril committed
36
    std::string hostName = "10.42.00.15:801";
37
38
39
40
41
42
    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
43
            ros::Duration(1.0).sleep();
44
        } else {
bucyril's avatar
bucyril committed
45
46
47
48
            ROS_INFO("Connected successfully");
        }
    }

49
50
    //set data stream parameters
    client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull);
bucyril's avatar
bucyril committed
51

52
53
54
55
    client.EnableSegmentData();
    client.EnableMarkerData();
    client.EnableUnlabeledMarkerData();
    client.EnableDeviceData();
bucyril's avatar
bucyril committed
56

57
58
59
60
    // Set the global up axis
    client.SetAxisMapping(Direction::Forward,
            Direction::Left,
            Direction::Up);
bucyril's avatar
bucyril committed
61

62
63
64
65
66
67
    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
68

69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
        unsigned int subjectCount = client.GetSubjectCount().SubjectCount;

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

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

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

            //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
100
101
            }

102
103
104
105
106
107
108
109
110
111
112
113
            //build and publish message
            d_fall_pps::ViconData viconData;
            viconData.crazyflieName = subjectName;
            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;

            viconDataPublisher.publish(viconData);
bucyril's avatar
bucyril committed
114
115
        }

116
    }
bucyril's avatar
bucyril committed
117

118
119
120
121
    client.DisableSegmentData();
    client.DisableMarkerData();
    client.DisableUnlabeledMarkerData();
    client.DisableDeviceData();
bucyril's avatar
bucyril committed
122

123
    client.Disconnect();
Philipp Friedli's avatar
Philipp Friedli committed
124
}