Commit bb6ce12d authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Various adaptations for filtering approaches

parent f9b9cd03
......@@ -19,11 +19,14 @@
#include <fstream>
#include <std_msgs/Float32MultiArray.h>
#include "std_msgs/Int32.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
#define UWB_NUM_ANCHORS 6
namespace d_fall_pps
{
// Loads the parameters of the node handle
......@@ -45,6 +48,8 @@ namespace d_fall_pps
// Callback function to handle incoming UWB data
void UWBDataCallback(const CrazyflieData &data);
void DistancesCallback(const std_msgs::Float32MultiArray& distances);
}
#endif // UWBANALYSIS_H included
......@@ -59,6 +59,8 @@ namespace d_fall_pps
// updates KalmanStates for given state
void updateKalmanState(KalmanState& state, double measurment);
void filterDistances(double* distances);
// loads Settings from UWBManagerService
void loadUWBSettings();
......@@ -86,6 +88,8 @@ namespace d_fall_pps
// calculatse distances from passed microseconds
float tiagosAwesomeFormula(uint32_t time);
void publishLog(const std_msgs::UInt32MultiArray& distances, const double* dist);
}
#endif // UWBDATAPUBLISHER_H included
......@@ -41,6 +41,7 @@ bool uwb_started = false;
std::string logPath = "/home/uwbclient01/Log/";
std::ofstream viconFile;
std::ofstream uwbFile;
std::ofstream distanceFile;
std::chrono::high_resolution_clock::time_point startTime;
......@@ -66,6 +67,7 @@ int main(int argc, char* argv[])
ROS_INFO_STREAM("[UWBAnalysis] subscribed to Vicon");
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
ROS_INFO_STREAM("[UWBAnalysis] subscribed to UWB");
ros::Subscriber DistancesSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/Distances", 10, DistancesCallback);
ROS_INFO_STREAM("[UWBAnalysis] Logging started!");
......@@ -140,6 +142,13 @@ void d_fall_pps::prepareLogs()
return;
}
distanceFile.open(logPath + "Distances.vlog", std::ofstream::out | std::ofstream::trunc);
if (!distanceFile)
{
ROS_ERROR("Error opening Distance log");
return;
}
ROS_INFO_STREAM("[UWBAnalysis] Log files prepared.");
}
......@@ -176,3 +185,21 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
writeToLog(viconFile, timestamp, CFData_vicon);
writeToLog(uwbFile, timestamp, CFData_uwb);
}
void d_fall_pps::DistancesCallback(const std_msgs::Float32MultiArray& distances)
{
std::chrono::high_resolution_clock::time_point cbTime = std::chrono::high_resolution_clock::now();
long timestamp = std::chrono::duration_cast<std::chrono::milliseconds>(cbTime - startTime).count();
distanceFile << timestamp << ", ";
for(int i = 0; i < 2 * UWB_NUM_ANCHORS; ++i)
{
distanceFile << distances.data[i];
if(i != 2 * UWB_NUM_ANCHORS - 1)
distanceFile << ", ";
else
distanceFile << "\n";
}
}
......@@ -44,18 +44,19 @@ rosbag::Bag bag;
ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher UWBDataPublisher;
ros::Publisher UWBLogPublisher;
double rollPitchYaw[3];
UWBAnchorArray anchors;
double anchorPositions[3*UWB_NUM_ANCHORS];
double prevDistances[UWB_NUM_ANCHORS] = {0};
KalmanState xState;
KalmanState yState;
KalmanState zState;
KalmanState kalmanDist[UWB_NUM_ANCHORS];
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBDataPublisher");
......@@ -77,6 +78,7 @@ int main(int argc, char* argv[])
ros::Subscriber uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback);
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 10);
UWBLogPublisher = nodeHandle.advertise<std_msgs::Float32MultiArray>("Distances", 1);
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
......@@ -162,16 +164,12 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
dist[i] = distances.data[i] / 100.0; //in dm
if(distances.data[i] == 0)
{
dist[i] = prevDistances[i];
ROS_WARN("[UWBDataPublisher] bad reading from Anchor %i", i);
}
else
prevDistances[i] = dist[i];
ROS_WARN("Bad reading from Anchor %i", i+1);
}
//filterDistances(dist);
IndoorNewton(anchorPositions, dist, result, 50);
updateKalmanState(xState, result[0] / 10.0);
......@@ -188,6 +186,40 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
xyz[0] = result[0] / 10.0;
xyz[1] = result[1] / 10.0;
xyz[2] = result[2] / 10.0;
publishLog(distances, dist);
}
void d_fall_pps::publishLog(const std_msgs::UInt32MultiArray& distances, const double* dist)
{
std_msgs::Float32MultiArray msg;
msg.data = {};
for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
{
msg.data.push_back(distances.data[i] / 100.0);
}
for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
{
msg.data.push_back(dist[i]);
}
/* for(int i = 0; i < 2 * UWB_NUM_ANCHORS; ++i)
ROS_WARN("%i, %f", i, msg.data[i]);*/
UWBLogPublisher.publish(msg);
}
void d_fall_pps::filterDistances(double* distances)
{
for (unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
if(distances[i] != 0)
updateKalmanState(kalmanDist[i], distances[i]);
distances[i] = kalmanDist[i].x;
}
}
void d_fall_pps::initKalmanStates()
......@@ -212,6 +244,14 @@ void d_fall_pps::initKalmanStates()
zState.q = 0.005;
zState.r = 6;
zState.p = 0;
for(unsigned int i = 0; i < UWB_NUM_ANCHORS; i++)
{
kalmanDist[i].x = 0;
kalmanDist[i].q = 0.005;
kalmanDist[i].r = 1;
kalmanDist[i].p = 0;
}
}
void d_fall_pps::updateKalmanState(KalmanState& state, double measurment)
......
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