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

added msg type for Anchor Positions

parent ee14d1f1
......@@ -132,6 +132,8 @@ add_message_files(
Setpoint.msg
CrazyflieEntry.msg
CrazyflieDB.msg
UWBAnchor.msg
UWBAnchorArray.msg
#----------------------------------------------------------------------
DebugMsg.msg
)
......
......@@ -19,6 +19,8 @@
#include "std_msgs/UInt32MultiArray.h"
#include <vector>
#include <stdint.h>
#include "d_fall_pps/UWBAnchor.h"
#include "d_fall_pps/UWBAnchorArray.h"
#include "ros/ros.h"
......@@ -42,9 +44,12 @@ namespace d_fall_pps
// calculates coordinates from input distances
void calculateXYZ(const std::vector<float> &distances, int distLen, double(&xyz)[3]);
// Callback functions to handle times information from CF
// Callback function to handle times information from CF
void cfTimesCallback(const std_msgs::UInt32MultiArray &times);
// calculatse distances from passed microseconds
float tiagosAwesomeFormula(uint32_t time);
// Callback function to handle new anchor positions
void anchorPosCallback(const UWBAnchorArray &newAnchors);
}
\ No newline at end of file
uint32 id
float32 x
float32 y
float32 z
\ No newline at end of file
uint32 length
UWBAnchor[] data
\ No newline at end of file
......@@ -45,6 +45,8 @@ double rollPitchYaw[3];
bool onboardPosition;
bool useTiagoData;
UWBAnchorArray anchors;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBDataPublisher");
......@@ -79,6 +81,8 @@ int main(int argc, char* argv[])
}
}
ros::Subscriber anchorPosSubscriber = namespaceNodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
......@@ -177,6 +181,8 @@ void d_fall_pps::calculateXYZ(const std::vector<float> &distances, int distLen,
{
//add algorithm from forces - paul
return;
}
......@@ -212,4 +218,12 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
float d_fall_pps::tiagosAwesomeFormula(uint32_t time)
{
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