Commit 4f836caa authored by bucyril's avatar bucyril
Browse files

ViconDataNode now working, added license, started PPSClient

parent d7afa9a4
......@@ -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 ##
#############
......
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
string crazyflieName
float64 x
float64 y
float64 z
float64 roll
float64 pitch
float64 yaw
float64 acquiringTime
// 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");
}
......@@ -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();
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment