Commit 4bedcb75 authored by michaero's avatar michaero
Browse files

Added UWBDataPublisher, implemented in LocalizationServer, need to test

parent f90716b1
{
"files.associations": {
"iterator": "cpp",
"string": "cpp",
"string_view": "cpp",
"vector": "cpp"
}
}
\ No newline at end of file
......@@ -13,11 +13,11 @@
#define LOCALIZATIONSERVER_H
#include "std_msgs/Int32.h"
#include "std_msgs/Float32MultiArray.h"
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
namespace d_fall_pps
{
......@@ -33,14 +33,8 @@ 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)
// Callback function to handle incoming UWB data
void UWBDataCallback(const CrazyflieData &data);
}
#endif // LOCALIZATIONSERVER_H included
\ No newline at end of file
// ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
// 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 "std_msgs/Int32.h"
#include "std_msgs/Float32MultiArray.h"
#include "ros/ros.h"
namespace d_fall_pps
{
//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)
// Loads the current context of the client
void loadCrazyflieContext();
// Loads the parameters of the node handle
bool loadParameters(ros::NodeHandle &nodeHandle);
}
\ No newline at end of file
......@@ -27,11 +27,6 @@ using namespace d_fall_pps;
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
// values for safetyCheck
bool strictSafety;
double[3] rollPitchYaw;
// Self explanatory variables
ros::ServiceClient centralManager;
ros::Publisher localizationPublisher;
......@@ -55,8 +50,8 @@ 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);
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("/UWBDataPublisher/UWBData",100,UWBDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to UWB");
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
......@@ -119,51 +114,11 @@ 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)
// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
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);
bag.write("UWBData", ros::Time::now(), data);
}
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
......
// ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
// 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 <string.h>
#include "DataStreamClient.h"
#include "ros/ros.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CMQuery.h"
#include "UWBDataPublisher"
using namespace d_fall_pps;
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
CrazyflieContext context;
rosbag::Bag bag;
ros::Publisher UWBDataPublisher;
double[3] rollPitchYaw;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBDataPublisher");
ros::NodeHandle nodeHandle("~");
ros::Time::init();
if(!loadParameters(nodeHandle))
return -1;
loadCrazyflieContext();
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
ros::Subscriber cfXYZSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
ros::Subscriber cfDistancesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
bag.close();
return 0;
}
//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; //???
UWBDataPublisher.publish(data);
}
//Callback function to handle distance information from CF
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
{
CrazyflieData data;
double[3] arrXYZ = [0,0,0];
calculateXYZ((double)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);
}
//calculates position from anchor locations and distances -> forces algorithm?
void calculateXYZ(const double[] &distances, double(&xyz)[3])
{
//add algorithm from forces - paul
return;
}
// Loads the current context of the client
void d_fall_pps::loadCrazyflieContext()
{
CMQuery contextCall;
contextCall.request.studentID = clientID;
centralManager.waitForExistence(ros::Duration(-1));
if(centralManager.call(contextCall))
{
context =contextCall.response.crazyflieContext;
ROS_INFO_STREAM("LocalizationServer context updated");
}
}
// Loads the parameters of the node handle
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
if(!nodeHandle.getParam("clientID", clientID))
{
ROS_ERROR("Failed to get clientID!");
return false;
}
return true;
}
\ No newline at end of file
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