Commit 0d8c41fb authored by tiagos's avatar tiagos
Browse files

Merge branch 'uwbmaster' of https://gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System into uwbmaster

parents 162fac4f f9b9cd03
......@@ -27,6 +27,9 @@ namespace d_fall_pps
// Loads the current context of the client
void loadCrazyflieContext();
// loads Settings from UWBManagerService
void loadUWBSettings();
// Callback function to handle incoming Vicon data
void viconDataCallback(const ViconData &viconData);
......
......@@ -14,6 +14,9 @@
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef UWBDATAPUBLISHER_H
#define UWBDATAPUBLISHER_H
#include <vector>
#include "ros/ros.h"
......@@ -33,9 +36,32 @@
namespace d_fall_pps
{
typedef struct
{
double q; // process noise covariance
double r; // measurment noise covariance > 0
double x; // state space variable
double p; // estimation error covariance
double k; // gain
} KalmanState;
// Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32& msg);
// Callback function to handle change in UWB settings
void uwbChangedCallback(const std_msgs::Int32 &msg);
// Initialises KalmanStates for x, y, z
void initKalmanStates();
// updates KalmanStates for given state
void updateKalmanState(KalmanState& state, double measurment);
// loads Settings from UWBManagerService
void loadUWBSettings();
//Callback function to handle roll pitch yaw information from CF
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
......@@ -60,7 +86,6 @@ namespace d_fall_pps
// calculatse distances from passed microseconds
float tiagosAwesomeFormula(uint32_t time);
// Callback function to handle new anchor positions
void anchorPosCallback(const UWBAnchorArray &newAnchors);
}
#endif // UWBDATAPUBLISHER_H included
......@@ -18,6 +18,7 @@
#include "LocalizationServer.h"
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
......@@ -30,13 +31,13 @@ int clientID;
// Self explanatory variables
ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher localizationPublisher;
CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB = true;
bool useUWB = true;
//bool useUWB = false;
bool useUWB = false;
CrazyflieData CFData_vicon;
......@@ -56,7 +57,9 @@ int main(int argc, char* argv[])
loadCrazyflieContext();
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
loadUWBSettings();
// subscribe to the global vicon publisher and local uwb publisher
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
......@@ -111,6 +114,26 @@ void d_fall_pps::loadCrazyflieContext()
}
}
void d_fall_pps::loadUWBSettings()
{
Anchors a;
if(uwbManager.call(a))
{
useUWB = a.response.enableUWB;
}
else
{
useUWB = false;
ROS_ERROR("[LocalizationServer] Could not update UWB Settings!");
}
if(useUWB)
ROS_WARN("[LocalizationServer] UWB activated!");
else
ROS_WARN("[LocalizationServer] UWB deactivated!");
}
void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
......@@ -139,8 +162,8 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
CrazyflieData CFData_uwb = data;
/* CFData_uwb.x = CFData_vicon.x;
CFData_uwb.y = CFData_vicon.y;
CFData_uwb.z = CFData_vicon.z;*/
CFData_uwb.y = CFData_vicon.y;*/
CFData_uwb.z = CFData_vicon.z;
CFData_uwb.roll = CFData_vicon.roll;
CFData_uwb.pitch = CFData_vicon.pitch;
CFData_uwb.yaw = CFData_vicon.yaw;
......@@ -158,9 +181,19 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
void d_fall_pps::uwbChangedCallback(const std_msgs::Int32 &msg)
{
enableUWB = msg.data;
Anchors a;
if(enableUWB)
if(uwbManager.call(a))
{
useUWB = a.response.enableUWB;
}
else
{
useUWB = false;
ROS_ERROR("[LocalizationServer] Could not update UWB Settings!");
}
if(useUWB)
ROS_WARN("[LocalizationServer] UWB activated!");
else
ROS_WARN("[LocalizationServer] UWB deactivated!");
......
......@@ -69,7 +69,7 @@ int main(int argc, char* argv[])
ROS_INFO_STREAM("[UWBAnalysis] Logging started!");
ros::Rate rate(25); // 50 Hz
/*ros::Rate rate(25); // 50 Hz
while (ros::ok())
{
......@@ -83,7 +83,9 @@ int main(int argc, char* argv[])
ros::spinOnce();
rate.sleep();
}
}*/
ros::spin();
ROS_WARN("[UWBAnalysis] Logging stoped!");
......@@ -168,4 +170,9 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
CFData_uwb = data;
uwb_started = true;
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();
writeToLog(viconFile, timestamp, CFData_vicon);
writeToLog(uwbFile, timestamp, CFData_uwb);
}
......@@ -23,6 +23,7 @@
#include <rosbag/bag.h>
#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
......@@ -30,21 +31,6 @@
#include "UWBDataPublisher.h"
//#include "DataStreamClient.h"
//#include "std_msgs/Float32.h"
//#include "std_msgs/Float32MultiArray.h"
using namespace d_fall_pps;
// This is a test comment to check wtf git does
......@@ -56,14 +42,19 @@ CrazyflieContext context;
rosbag::Bag bag;
ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher UWBDataPublisher;
double rollPitchYaw[3];
bool onboardPosition = false;
bool useTiagoData = true;
UWBAnchorArray anchors;
double anchorPositions[3*UWB_NUM_ANCHORS];
double prevDistances[UWB_NUM_ANCHORS] = {0};
KalmanState xState;
KalmanState yState;
KalmanState zState;
int main(int argc, char* argv[])
{
......@@ -76,11 +67,12 @@ int main(int argc, char* argv[])
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
ros::Subscriber dbChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
// Subscribe to the anchor positions and all relevant Data from crazyradio
//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 = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback);
......@@ -91,6 +83,8 @@ int main(int argc, char* argv[])
std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
bag.open(record_file, rosbag::bagmode::Write);
initKalmanStates();
ROS_INFO_STREAM("[UWBDataPublisher] started!");
ros::spin();
......@@ -104,6 +98,32 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg)
d_fall_pps::loadCrazyflieContext();
}
void d_fall_pps::uwbChangedCallback(const std_msgs::Int32& msg)
{
d_fall_pps::loadUWBSettings();
}
void d_fall_pps::loadUWBSettings()
{
Anchors a;
if(uwbManager.call(a))
{
anchors = a.response.anchorArray;
}
else
{
ROS_ERROR("[UWBDataPublisher] Could not update UWB Settings!");
}
for(int i = 0; i < UWB_NUM_ANCHORS; ++i)
{
anchorPositions[3*i] = anchors.data[i].x;
anchorPositions[3*i + 1] = anchors.data[i].y;
anchorPositions[3*i + 2] = anchors.data[i].z;
}
}
//Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
......@@ -131,22 +151,11 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances
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_msgs::UInt32MultiArray& distances, double(&xyz)[3])
{
//double positions[3*UWB_NUM_ANCHORS] = { 12,0,0, 28,0,10, 28,20,0, 15,30,10, 0,22,0, 0,6,0 };
double positions[3*UWB_NUM_ANCHORS] = {
-1.2, -14.0, 5.8,
14.9, -14.0, 15.9,
14.7, 5.8, 5.8,
1.1, 14.6, 15.9,
-13.5, 7.6, 5.8,
-13.4, -8.4, 5.8
};
double dist[UWB_NUM_ANCHORS];
double result[3];
......@@ -155,41 +164,62 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
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];
}
IndoorNewton(positions, dist, result, 50);
xyz[0] = result[0] / 10.0;
xyz[1] = result[1] / 10.0;
xyz[2] = result[2] / 10.0;
/*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;
IndoorNewton(anchorPositions, dist, result, 50);
const double dx = -0.6;
const double dy = -1.4;
const double dz = -0.35;
updateKalmanState(xState, result[0] / 10.0);
updateKalmanState(yState, result[1] / 10.0);
updateKalmanState(zState, result[2] / 10.0);
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)));
// Use this for Kalmanfiltered position
//xyz[0] = xState.x;
//xyz[1] = yState.x;
//xyz[2] = zState.x;
x += dx;
y += dy;
z += dz;
// Use this for unfiltered position
xyz[0] = result[0] / 10.0;
xyz[1] = result[1] / 10.0;
xyz[2] = result[2] / 10.0;
}
xyz[0] = x;
xyz[1] = y;
xyz[2] = z;
void d_fall_pps::initKalmanStates()
{
// Init x state
// TODO Update
xState.x = 0;
xState.q = 0.005;
xState.r = 6;
xState.p = 0;
// Init y state
// TODO Update
yState.x = 0;
yState.q = 0.005;
yState.r = 6;
yState.p = 0;
// Init z state
// TODO Update
zState.x = 0;
zState.q = 0.005;
zState.r = 6;
zState.p = 0;
}
return;*/
void d_fall_pps::updateKalmanState(KalmanState& state, double measurment)
{
state.p += state.q;
state.k = state.p / (state.p + state.r);
state.x += state.k * (measurment - state.x);
state.p *= (1 - state.k);
}
// Loads the current context of the client
......@@ -225,11 +255,3 @@ float d_fall_pps::tiagosAwesomeFormula(uint32_t ticks)
return ticks * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
//return (time * 75.0 * (1.0 / (128.0 * 499.2)));
}
// Callback function to handle new anchor positions
void d_fall_pps::anchorPosCallback(const UWBAnchorArray &newAnchors)
{
anchors = newAnchors;
return;
}
\ 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