Commit 5b91247b authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Position Data can now be either Vicon or UWB

parent 6561ea96
......@@ -41,10 +41,10 @@ namespace d_fall_pps
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
//Callback funtion to hande x y z information from CF
void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ);
//void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ);
//Callback function to handle distance information from CF
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances);
//void cfDistancesCallback(const std_msgs::Float32MultiArray &distances);
// Loads the current context of the client
void loadCrazyflieContext();
......@@ -53,10 +53,11 @@ namespace d_fall_pps
bool loadParameters(ros::NodeHandle &nodeHandle);
// calculates coordinates from input distances
void calculateXYZ(const std::vector<float> &distances, double(&xyz)[3]);
//void calculateXYZ(const std::vector<float> &distances, double(&xyz)[3]);
void calculateXYZ(const std_msgs::UInt32MultiArray &distances, double(&xyz)[3]);
// Callback function to handle times information from CF
void cfTimesCallback(const std_msgs::UInt32MultiArray &times);
void cfDistancesCallback(const std_msgs::UInt32MultiArray &distances);
// calculatse distances from passed microseconds
float tiagosAwesomeFormula(uint32_t time);
......
......@@ -34,7 +34,7 @@ CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB = true;
bool useUWB = false;
bool useUWB = true;
bool onboardPosition = false;
int main(int argc, char* argv[])
......@@ -57,9 +57,10 @@ int main(int argc, char* argv[])
// subscribe to the global vicon publisher and local uwb publisher
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
ros::Subscriber UWBSubscriber;
if(enableUWB)
{
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
}
......
......@@ -17,13 +17,16 @@
#include <stdlib.h>
#include <string.h>
//#include "ros/ros.h"
#include <ros/package.h>
#include <rosbag/bag.h>
#include "DataStreamClient.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"
......@@ -59,7 +62,6 @@ int main(int argc, char* argv[])
ros::init(argc, argv, "UWBDataPublisher");
ros::NodeHandle nodeHandle("~");
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
//ros::Time::init();
if(!loadParameters(nodeHandle))
return -1;
......@@ -72,21 +74,9 @@ int main(int argc, char* argv[])
//ros::Subscriber anchorPosSubscriber = namespaceNodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
ros::Subscriber anchorPosSubscriber = nodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
ros::Subscriber uwbLocationSubscriber;
if(onboardPosition)
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
else
{
if(useTiagoData)
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
else
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
}
ros::Subscriber uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback);
// TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 10);
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
......@@ -114,60 +104,13 @@ void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
rollPitchYaw[2] = arrRPY.data[2];
}
//Callback funtion to hande x y z information from CF
void d_fall_pps::cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
{
CrazyflieData data;
data.crazyflieName = context.crazyflieName;
data.x = (double)arrXYZ.data[0];
data.y = (double)arrXYZ.data[1];
data.z = (double)arrXYZ.data[2];
data.roll = rollPitchYaw[0];
data.pitch = rollPitchYaw[1];
data.yaw = rollPitchYaw[2];
data.occluded = false;
data.acquiringTime = 0; //???
UWBDataPublisher.publish(data);
}
//Callback function to handle distance information from CF
void d_fall_pps::cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
{
CrazyflieData data;
double arrXYZ[3] = {0};
calculateXYZ(distances.data, arrXYZ); //TODO: Implement!!!
data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0];
data.y = arrXYZ[1];
data.z = arrXYZ[2];
data.roll = rollPitchYaw[0];
data.pitch = rollPitchYaw[1];
data.yaw = rollPitchYaw[2];
data.occluded = false;
data.acquiringTime = 0; //???
UWBDataPublisher.publish(data);
}
// Callback functions to handle times information from CF
void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances)
{
CrazyflieData data;
double arrXYZ[3] = {0};
std::vector<float> distances;
for (int i = 0; i < UWB_NUM_ANCHORS; i++)
distances.push_back(tiagosAwesomeFormula(times.data[i]));
calculateXYZ(distances, arrXYZ); //TODO: Implement!!!
calculateXYZ(distances, arrXYZ);
data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0];
......@@ -180,14 +123,39 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
data.acquiringTime = 0; //???
UWBDataPublisher.publish(data);
//ROS_INFO("[UWBDataPublisher]\n\tx: %f\n\ty: %f\n\tz: %f", data.x, data.y, data.z);
}
//calculates position from anchor locations and distances -> forces algorithm?
void d_fall_pps::calculateXYZ(const std::vector<float> &distances, double(&xyz)[3])
void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, double(&xyz)[3])
{
//add algorithm from forces - paul
double r1, r2, r3, x, y, z;
const double tri_d = 1.8;
const double tri_i = 0.0;
const double tri_j = 2.7;
const double dx = -0.6;
const double dy = -1.4;
const double dz = -0.35;
r1 = distances.data[0]/1000.0;
r2 = distances.data[1]/1000.0;
r3 = distances.data[2]/1000.0;
x = ((r1 * r1) - (r2 * r2) + (tri_d * tri_d))/(2 * tri_d);
y = ((r1 * r1) - (r3 * r3) + (tri_i * tri_i) + (tri_j * tri_j))/(2 * tri_j) - (tri_i * x)/tri_j;
z = std::sqrt(std::abs((r1 * r1) - (x * x) - (y * y)));
x += dx;
y += dy;
z += dz;
xyz[0] = x;
xyz[1] = y;
xyz[2] = z;
return;
}
......@@ -220,9 +188,9 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
}
// calculatse distances from passed microseconds
float d_fall_pps::tiagosAwesomeFormula(uint32_t time)
float d_fall_pps::tiagosAwesomeFormula(uint32_t ticks)
{
return time * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
return ticks * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
//return (time * 75.0 * (1.0 / (128.0 * 499.2)));
}
......
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