ViconDataNode now working, added license, started PPSClient

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -8,6 +8,7 @@ project(d_fall_pps)
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
+  message_generation
   roscpp
   rospy
   std_msgs
@@ -53,6 +54,11 @@ find_package(catkin REQUIRED COMPONENTS
 #   Message2.msg
 # )
 
+add_message_files(
+  FILES
+  ViconData.msg
+)
+
 ## Generate services in the 'srv' folder
 # add_service_files(
 #   FILES
@@ -72,6 +78,10 @@ find_package(catkin REQUIRED COMPONENTS
 #   DEPENDENCIES
 #   std_msgs
 # )
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -136,6 +146,7 @@ include_directories(
 ## The recommended prefix ensures that target names across packages don't collide
 # add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
 add_executable(ViconDataNode src/ViconDataNode.cpp)
+add_executable(PPSClient src/PPSClient.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -157,6 +168,8 @@ find_library(VICON_LIBRARY ViconDataStreamSDK_CPP HINTS lib/vicon)
 target_link_libraries(ViconDataNode ${catkin_LIBRARIES})
 target_link_libraries(ViconDataNode ${VICON_LIBRARY})
 
+target_link_libraries(PPSClient ${catkin_LIBRARIES})
+
 #############
 ## Install ##
 #############

pps_ws/src/d_fall_pps/msg/ControllerOutput.msg

+this is copied from dusan and might therefore need modification
+also you need to add this file in the CMakeLists.txt
+
+
+float32 roll
+float32 pitch
+float32 yaw
+uint16 thrust
+uint16 motorCmd1
+uint16 motorCmd2
+uint16 motorCmd3
+uint16 motorCmd4
+uint8 onboardControllerType
+

pps_ws/src/d_fall_pps/msg/ViconData.msg

+string crazyflieName
+float64 x
+float64 y
+float64 z
+float64 roll
+float64 pitch
+float64 yaw
+float64 acquiringTime

pps_ws/src/d_fall_pps/src/PPSClient.cpp

+//    ROS node that manages the student's setup.
+//    Copyright (C) 2017  Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+//    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.
+//
+//    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/>.
+
+#include "ros/ros.h"
+
+int main(int argc, char* argv[]) {
+    ROS_INFO_STREAM("fdsafsfdsalj");
+}

pps_ws/src/d_fall_pps/src/ViconDataNode.cpp

@@ -12,136 +12,108 @@
 #include <string.h>
 #include "DataStreamClient.h"
 #include "ros/ros.h"
+#include "d_fall_pps/ViconData.h"
 
 using namespace ViconDataStreamSDK::CPP;
 
-void connect(Client* client) {
+int main(int argc, char* argv[]) {
+    ros::init(argc, argv, "ViconDataNode");
+
+    ros::NodeHandle nodeHandle("~");
+    ros::Time::init();
+
+    ros::Publisher viconDataPublisher =
+            nodeHandle.advertise<d_fall_pps::ViconData>("ViconData", 1);
+
+    Client client;
+
+    //connect client to Vicon computer
     std::string hostName = "10.42.00.15:801";
-	ROS_INFO_STREAM("Connecting to " << hostName << " ...");
-    while( !client->IsConnected().Connected )
-    {
-        bool ok =( client->Connect( hostName ).Result == Result::Success );
-
-        if(!ok)
-        {
-            ROS_ERROR("Warning - connection failed...");
+    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...");
             ros::Duration(1.0).sleep();
-        }
-        else
-        {
+        } else {
             ROS_INFO("Connected successfully");
         }
     }
-}
 
-void setupDataStream(Client* client) {
-	// Enable some different data types
-        ROS_INFO_STREAM("ClientPull stream mode enabled3");
-        client->EnableSegmentData();
-        client->EnableMarkerData();
-        client->EnableUnlabeledMarkerData();
-        client->EnableDeviceData();
-        ROS_INFO_STREAM("ClientPull stream mode enabled4");
-
-        // Set the streaming mode
-        client->SetStreamMode( ViconDataStreamSDK::CPP::StreamMode::ClientPull );
-        ROS_INFO_STREAM("ClientPull stream mode enabled");
-
-        // Set the global up axis
-        client->SetAxisMapping( Direction::Forward,
-                                 Direction::Left,
-                                 Direction::Up ); // Z-up
-        // client->SetGlobalUpAxis( Direction::Forward,
-        //                           Direction::Up,
-        //                           Direction::Right ); // Y-up
-}
-
-int main( int argc, char* argv[] )
-{
-    ros::init(argc, argv, "ViconDataStreamSDK");
-    ROS_INFO_STREAM("Okili dokili");
-
-    ros::NodeHandle nodeHandle("~");
-    ros::Time::init();
+    //set data stream parameters
+    client.SetStreamMode(ViconDataStreamSDK::CPP::StreamMode::ClientPull);
 
-    Client client;
-    connect(&client);
-    setupDataStream(&client);
-
-    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();
-            }
-            
-            // Count the number of subjects
-            unsigned int subjectCount = client.GetSubjectCount().SubjectCount;
+    client.EnableSegmentData();
+    client.EnableMarkerData();
+    client.EnableUnlabeledMarkerData();
+    client.EnableDeviceData();
 
-            // //Process the data and publish on topic
-             for(int index = 0 ; index < subjectCount ; index++ )
-             {
-             	std::string subjName = client.GetSubjectName( index ).SubjectName;
-             	std::string segName = client.GetSegmentName( subjName, index ).SegmentName;
+    // Set the global up axis
+    client.SetAxisMapping(Direction::Forward,
+            Direction::Left,
+            Direction::Up);
 
-                 ROS_INFO_STREAM(subjName);
-                 ROS_INFO_STREAM(segName);
-                 ROS_INFO_STREAM("------");
-             }
+    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();
+        }
 
-    }
-/*
-        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();
+        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;
             }
 
-            //Get frame rate
-            //            Output_GetFrameRate OutputFrameRate = client.GetFrameRate ();
-            //            if(OutputFrameRate.Result==Result::Success)
-            //                std::cout<<"Frame Rate= "<<OutputFrameRate.FrameRateHz<<std::endl;
-            //            else
-            //                std::cout<<"Frame Rate not fetched ERROR"<<std::endl;
-
-            // //Get latency
-            // OutputLatencyTotal = client.GetLatencyTotal();
-            // if(OutputLatencyTotal.Result==Result::Success)
-            //     totalViconLatency=OutputLatencyTotal.Total;
-            // else
-            //     totalViconLatency=0;
-            // //std::cout<<"Total latency: "<<totalViconLatency;
-            // dataAcquiringTime=loopStartTime-ros::Duration(totalViconLatency);
-
-
-            // Count the number of subjects
-            int subjectCount = client.GetSubjectCount().SubjectCount;
-            ROS_INFO_STREAM();
-
-            // //Process the data and publish on topic
-            // for( SubjectIndex = 0 ; SubjectIndex < SubjectCount ; ++SubjectIndex )
-            // {
-
-            //     // Get the subject's name
-            //     subjectName = client.GetSubjectName( subjectIndex ).SubjectName;
-            //     segmentName = client.GetSegmentName( subjectName, subjectIndex ).SegmentName;
-            //     ROS_INFO_STREAM();
-            // }
+            //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);
         }
 
-        client.DisableSegmentData();
-        client.DisableMarkerData();
-        client.DisableUnlabeledMarkerData();
-        client.DisableDeviceData();
+    }
 
+    client.DisableSegmentData();
+    client.DisableMarkerData();
+    client.DisableUnlabeledMarkerData();
+    client.DisableDeviceData();
 
-        // Disconnect and dispose
-        ROS_WARN_STREAM(" Disconnecting..." << std::endl);
-        client.Disconnect();
-        ROS_WARN_STREAM(" Disconnected" << std::endl);
-*/
+    client.Disconnect();
 }