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 ...@@ -27,6 +27,9 @@ namespace d_fall_pps
// Loads the current context of the client // Loads the current context of the client
void loadCrazyflieContext(); void loadCrazyflieContext();
// loads Settings from UWBManagerService
void loadUWBSettings();
// Callback function to handle incoming Vicon data // Callback function to handle incoming Vicon data
void viconDataCallback(const ViconData &viconData); void viconDataCallback(const ViconData &viconData);
......
...@@ -14,6 +14,9 @@ ...@@ -14,6 +14,9 @@
// You should have received a copy of the GNU General Public License // You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef UWBDATAPUBLISHER_H
#define UWBDATAPUBLISHER_H
#include <vector> #include <vector>
#include "ros/ros.h" #include "ros/ros.h"
...@@ -33,9 +36,32 @@ ...@@ -33,9 +36,32 @@
namespace d_fall_pps 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 // Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32& msg); 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 //Callback function to handle roll pitch yaw information from CF
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY); void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
...@@ -60,7 +86,6 @@ namespace d_fall_pps ...@@ -60,7 +86,6 @@ namespace d_fall_pps
// calculatse distances from passed microseconds // calculatse distances from passed microseconds
float tiagosAwesomeFormula(uint32_t time); 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 @@ ...@@ -18,6 +18,7 @@
#include "LocalizationServer.h" #include "LocalizationServer.h"
#include "d_fall_pps/CMQuery.h" #include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h" #include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h" #include "d_fall_pps/CrazyflieContext.h"
...@@ -30,13 +31,13 @@ int clientID; ...@@ -30,13 +31,13 @@ int clientID;
// Self explanatory variables // Self explanatory variables
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher localizationPublisher; ros::Publisher localizationPublisher;
CrazyflieContext context; CrazyflieContext context;
rosbag::Bag bag; rosbag::Bag bag;
bool enableUWB = true; bool enableUWB = true;
bool useUWB = true; bool useUWB = false;
//bool useUWB = false;
CrazyflieData CFData_vicon; CrazyflieData CFData_vicon;
...@@ -56,7 +57,9 @@ int main(int argc, char* argv[]) ...@@ -56,7 +57,9 @@ int main(int argc, char* argv[])
loadCrazyflieContext(); loadCrazyflieContext();
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback); 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); ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
loadUWBSettings();
// subscribe to the global vicon publisher and local uwb publisher // subscribe to the global vicon publisher and local uwb publisher
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback); ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
...@@ -111,6 +114,26 @@ void d_fall_pps::loadCrazyflieContext() ...@@ -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) void d_fall_pps::viconDataCallback(const ViconData &viconData)
{ {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) 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) ...@@ -139,8 +162,8 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
CrazyflieData CFData_uwb = data; CrazyflieData CFData_uwb = data;
/* CFData_uwb.x = CFData_vicon.x; /* CFData_uwb.x = CFData_vicon.x;
CFData_uwb.y = CFData_vicon.y; CFData_uwb.y = CFData_vicon.y;*/
CFData_uwb.z = CFData_vicon.z;*/ CFData_uwb.z = CFData_vicon.z;
CFData_uwb.roll = CFData_vicon.roll; CFData_uwb.roll = CFData_vicon.roll;
CFData_uwb.pitch = CFData_vicon.pitch; CFData_uwb.pitch = CFData_vicon.pitch;
CFData_uwb.yaw = CFData_vicon.yaw; CFData_uwb.yaw = CFData_vicon.yaw;
...@@ -158,9 +181,19 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg) ...@@ -158,9 +181,19 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
void d_fall_pps::uwbChangedCallback(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!"); ROS_WARN("[LocalizationServer] UWB activated!");
else else
ROS_WARN("[LocalizationServer] UWB deactivated!"); ROS_WARN("[LocalizationServer] UWB deactivated!");
......
...@@ -69,7 +69,7 @@ int main(int argc, char* argv[]) ...@@ -69,7 +69,7 @@ int main(int argc, char* argv[])
ROS_INFO_STREAM("[UWBAnalysis] Logging started!"); ROS_INFO_STREAM("[UWBAnalysis] Logging started!");
ros::Rate rate(25); // 50 Hz /*ros::Rate rate(25); // 50 Hz
while (ros::ok()) while (ros::ok())
{ {
...@@ -83,7 +83,9 @@ int main(int argc, char* argv[]) ...@@ -83,7 +83,9 @@ int main(int argc, char* argv[])
ros::spinOnce(); ros::spinOnce();
rate.sleep(); rate.sleep();
} }*/
ros::spin();
ROS_WARN("[UWBAnalysis] Logging stoped!"); ROS_WARN("[UWBAnalysis] Logging stoped!");
...@@ -168,4 +170,9 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data) ...@@ -168,4 +170,9 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{ {
CFData_uwb = data; CFData_uwb = data;
uwb_started = true; 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 @@ ...@@ -23,6 +23,7 @@
#include <rosbag/bag.h> #include <rosbag/bag.h>
#include "d_fall_pps/CMQuery.h" #include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"
#include "d_fall_pps/CrazyflieData.h" #include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h" #include "d_fall_pps/CrazyflieContext.h"
...@@ -30,21 +31,6 @@ ...@@ -30,21 +31,6 @@
#include "UWBDataPublisher.h" #include "UWBDataPublisher.h"
//#include "DataStreamClient.h"
//#include "std_msgs/Float32.h"
//#include "std_msgs/Float32MultiArray.h"
using namespace d_fall_pps; using namespace d_fall_pps;
// This is a test comment to check wtf git does // This is a test comment to check wtf git does
...@@ -56,14 +42,19 @@ CrazyflieContext context; ...@@ -56,14 +42,19 @@ CrazyflieContext context;
rosbag::Bag bag; rosbag::Bag bag;
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher UWBDataPublisher; ros::Publisher UWBDataPublisher;
double rollPitchYaw[3]; double rollPitchYaw[3];
bool onboardPosition = false;
bool useTiagoData = true;
UWBAnchorArray anchors; 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[]) int main(int argc, char* argv[])
{ {
...@@ -76,11 +67,12 @@ int main(int argc, char* argv[]) ...@@ -76,11 +67,12 @@ int main(int argc, char* argv[])
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false); centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext(); 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 // 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 cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
ros::Subscriber uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback); ros::Subscriber uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/AnchorDistances", 10, cfDistancesCallback);
...@@ -91,6 +83,8 @@ int main(int argc, char* argv[]) ...@@ -91,6 +83,8 @@ int main(int argc, char* argv[])
std::string record_file = package_path + "log/UWBDataPublisherLog.bag"; std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
bag.open(record_file, rosbag::bagmode::Write); bag.open(record_file, rosbag::bagmode::Write);
initKalmanStates();
ROS_INFO_STREAM("[UWBDataPublisher] started!"); ROS_INFO_STREAM("[UWBDataPublisher] started!");
ros::spin(); ros::spin();
...@@ -104,6 +98,32 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg) ...@@ -104,6 +98,32 @@ void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg)
d_fall_pps::loadCrazyflieContext(); 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 //Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY) void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{ {
...@@ -131,22 +151,11 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances ...@@ -131,22 +151,11 @@ void d_fall_pps::cfDistancesCallback(const std_msgs::UInt32MultiArray &distances
data.acquiringTime = 0; //??? data.acquiringTime = 0; //???
UWBDataPublisher.publish(data); 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? //calculates position from anchor locations and distances -> forces algorithm?
void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, double(&xyz)[3]) 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 dist[UWB_NUM_ANCHORS];
double result[3]; double result[3];
...@@ -155,41 +164,62 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl ...@@ -155,41 +164,62 @@ void d_fall_pps::calculateXYZ(const std_msgs::UInt32MultiArray& distances, doubl
dist[i] = distances.data[i] / 100.0; //in dm dist[i] = distances.data[i] / 100.0; //in dm
if(distances.data[i] == 0) if(distances.data[i] == 0)
{
dist[i] = prevDistances[i];
ROS_WARN("[UWBDataPublisher] bad reading from Anchor %i", i); ROS_WARN("[UWBDataPublisher] bad reading from Anchor %i", i);
} }
else
prevDistances[i] = dist[i];
}
IndoorNewton(positions, dist, result, 50); IndoorNewton(anchorPositions, 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;
const double dx = -0.6; updateKalmanState(xState, result[0] / 10.0);
const double dy = -1.4; updateKalmanState(yState, result[1] / 10.0);
const double dz = -0.35; 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); // Use this for Kalmanfiltered position
y = ((r1 * r1) - (r3 * r3) + (tri_i * tri_i) + (tri_j * tri_j))/(2 * tri_j) - (tri_i * x)/tri_j; //xyz[0] = xState.x;
z = std::sqrt(std::abs((r1 * r1) - (x * x) - (y * y))); //xyz[1] = yState.x;
//xyz[2] = zState.x;
x += dx; // Use this for unfiltered position
y += dy; xyz[0] = result[0] / 10.0;
z += dz; xyz[1] = result[1] / 10.0;
xyz[2] = result[2] / 10.0;
}
xyz[0] = x; void d_fall_pps::initKalmanStates()
xyz[1] = y; {
xyz[2] = z; // 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 // Loads the current context of the client
...@@ -225,11 +255,3 @@ float d_fall_pps::tiagosAwesomeFormula(uint32_t ticks) ...@@ -225,11 +255,3 @@ float d_fall_pps::tiagosAwesomeFormula(uint32_t ticks)
return ticks * SPEED_OF_LIGHT / LOCO_DECK_FRQ; return ticks * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
//return (time * 75.0 * (1.0 / (128.0 * 499.2))); //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