From 4f836caae59de5a737c6d92b2b4ce38d20a967ae Mon Sep 17 00:00:00 2001
From: bucyril <bucyril@student.ethz.ch>
Date: Mon, 20 Mar 2017 15:52:57 +0100
Subject: [PATCH] ViconDataNode now working, added license, started PPSClient
---
pps_ws/src/d_fall_pps/CMakeLists.txt | 13 ++
.../src/d_fall_pps/msg/ControllerOutput.msg | 14 ++
pps_ws/src/d_fall_pps/msg/ViconData.msg | 8 +
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 21 ++
pps_ws/src/d_fall_pps/src/ViconDataNode.cpp | 198 ++++++++----------
5 files changed, 141 insertions(+), 113 deletions(-)
create mode 100644 pps_ws/src/d_fall_pps/msg/ControllerOutput.msg
create mode 100755 pps_ws/src/d_fall_pps/msg/ViconData.msg
create mode 100644 pps_ws/src/d_fall_pps/src/PPSClient.cpp
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 0e88d9b7..c2e763df 100644
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/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 ##
#############
diff --git a/pps_ws/src/d_fall_pps/msg/ControllerOutput.msg b/pps_ws/src/d_fall_pps/msg/ControllerOutput.msg
new file mode 100644
index 00000000..455f8557
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/ControllerOutput.msg
@@ -0,0 +1,14 @@
+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
+
diff --git a/pps_ws/src/d_fall_pps/msg/ViconData.msg b/pps_ws/src/d_fall_pps/msg/ViconData.msg
new file mode 100755
index 00000000..95d7180f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/ViconData.msg
@@ -0,0 +1,8 @@
+string crazyflieName
+float64 x
+float64 y
+float64 z
+float64 roll
+float64 pitch
+float64 yaw
+float64 acquiringTime
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
new file mode 100644
index 00000000..38b3134b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -0,0 +1,21 @@
+// 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");
+}
diff --git a/pps_ws/src/d_fall_pps/src/ViconDataNode.cpp b/pps_ws/src/d_fall_pps/src/ViconDataNode.cpp
index dece3b9f..61ba7e02 100644
--- a/pps_ws/src/d_fall_pps/src/ViconDataNode.cpp
+++ b/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();
}
--
GitLab