Commit f90716b1 authored by michaero's avatar michaero
Browse files

added cf radio subscribers to localizationServer, added callbacks publishing cfdata

parent e7a70c3c
......@@ -13,6 +13,7 @@
#define LOCALIZATIONSERVER_H
#include "std_msgs/Int32.h"
#include "std_msgs/Float32MultiArray.h"
#include "ros/ros.h"
......@@ -31,6 +32,15 @@ namespace d_fall_pps
// Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32 &msg);
//Callback function to handle roll pitch yaw information from CF
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
//Callback funtion to hande x y z information from CF
void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
//Callback function to handle distance information from CF
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
}
#endif // LOCALIZATIONSERVER_H included
\ No newline at end of file
......@@ -30,6 +30,8 @@ int clientID;
// values for safetyCheck
bool strictSafety;
double[3] rollPitchYaw;
// Self explanatory variables
ros::ServiceClient centralManager;
ros::Publisher localizationPublisher;
......@@ -53,6 +55,9 @@ int main(int argc, char* argv[])
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to Vicon");
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
......@@ -114,6 +119,53 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
}
}
//Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
rollPitchYaw = (double)arrRPY.data;
}
//Callback funtion to hande x y z information from CF
void 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; //???
localizationPublisher.publish(data);
bag.write("ViconData", ros::Time::now(), data);
}
//Callback function to handle distance information from CF
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
{
CrazyflieData data;
//arrXYZ = calculateXYZ(distances); //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; //???
localizationPublisher.publish(data);
bag.write("ViconData", ros::Time::now(), data);
}
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
d_fall_pps::loadCrazyflieContext();
......
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