Commit 37685fef authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Adapted UWBDataPublisher and CrazyRadio to be more generic

parent c28e866a
...@@ -79,6 +79,9 @@ CMD_CRAZYFLY_TAKE_OFF = 3 ...@@ -79,6 +79,9 @@ CMD_CRAZYFLY_TAKE_OFF = 3
CMD_CRAZYFLY_LAND = 4 CMD_CRAZYFLY_LAND = 4
CMD_CRAZYFLY_MOTORS_OFF = 5 CMD_CRAZYFLY_MOTORS_OFF = 5
# max Number of UWB Anchors
UWB_NUM_ANCHORS = 6
rp = RosPack() rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag' record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf') rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
...@@ -156,15 +159,18 @@ class PPSRadioClient: ...@@ -156,15 +159,18 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED) self.change_status_to(DISCONNECTED)
def _anchorTime_received_callback(self, timestamp, data, anchorTime): def _anchorTime_received_callback(self, timestamp, data, anchorTime):
times = UInt32MultiArray() ticks = UInt32MultiArray()
times.data = [data["UWB_Anchor_Pos.anchor0"], data["UWB_Anchor_Pos.anchor1"]]
ticks.data = []
for i in range(UWB_NUM_ANCHORS):
ticks.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
#For debug, to confirm times arrive... # for debugging only
print "dist0: %s" % (times.data[0] )#* 75.0 * (1.0 / (128.0 * 499.2))) #for i in range(UWB_NUM_ANCHORS):
print "dist1: %s" % (times.data[1] )#* 75.0 * (1.0 / (128.0 * 499.2))) # rospy.loginfo("Anchor " + str(i) + ": " + str(ticks.data[i]))
bag.write('anchorTime', times) bag.write('anchorTime', ticks)
uwb_location_pub.publish(times) uwb_location_pub.publish(ticks)
def _anchorDist_received_callback(self, timestamp, data, anchorDist): def _anchorDist_received_callback(self, timestamp, data, anchorDist):
distances = Float32MultiArray() distances = Float32MultiArray()
...@@ -287,8 +293,12 @@ class PPSRadioClient: ...@@ -287,8 +293,12 @@ class PPSRadioClient:
global anchordist_polling_period global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period") anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
self.anchorTime = LogConfig("AnchorTime", anchordist_polling_period) self.anchorTime = LogConfig("AnchorTime", anchordist_polling_period)
self.anchorTime.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
self.anchorTime.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t"); for i in range(UWB_NUM_ANCHORS):
self.anchorTime.add_variable("UWB_Anchor_Pos.anchor" + str(i), "uint32_t");
#self.anchorTime.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
#self.anchorTime.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
self._cf.log.add_config(self.anchorTime) self._cf.log.add_config(self.anchorTime)
if self.anchorTime.valid: if self.anchorTime.valid:
......
...@@ -14,18 +14,29 @@ ...@@ -14,18 +14,29 @@
// 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/>.
#include <vector>
//#include <stdint.h>
#include "ros/ros.h"
#include "std_msgs/Int32.h" #include "std_msgs/Int32.h"
#include "std_msgs/Float32MultiArray.h" #include "std_msgs/Float32MultiArray.h"
#include "std_msgs/UInt32MultiArray.h" #include "std_msgs/UInt32MultiArray.h"
#include <vector>
#include <stdint.h>
#include "d_fall_pps/UWBAnchor.h" #include "d_fall_pps/UWBAnchor.h"
#include "d_fall_pps/UWBAnchorArray.h" #include "d_fall_pps/UWBAnchorArray.h"
#include "ros/ros.h" #define UWB_NUM_ANCHORS 6
#define SPEED_OF_LIGHT 299792458.0
#define LOCO_DECK_FRQ (499.2e6 * 128)
namespace d_fall_pps 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 //Callback function to handle roll pitch yaw information from CF
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY); void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
......
...@@ -10,10 +10,7 @@ ...@@ -10,10 +10,7 @@
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI"> <node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> --> <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node> </node>
<node pkg="d_fall_pps" name="UWB_BaseStation" output="screen" type="UWB_BaseStation">
</node>
<node pkg="d_fall_pps" name="TeacherService" output="screen" type="TeacherService"> <node pkg="d_fall_pps" name="TeacherService" output="screen" type="TeacherService">
</node> </node>
......
...@@ -30,53 +30,48 @@ int clientID; ...@@ -30,53 +30,48 @@ int clientID;
// Self explanatory variables // Self explanatory variables
ros::ServiceClient centralManager; ros::ServiceClient centralManager;
ros::Publisher localizationPublisher; ros::Publisher localizationPublisher;
//std::string ros_namespace; // Needed?
CrazyflieContext context; CrazyflieContext context;
rosbag::Bag bag; rosbag::Bag bag;
bool enableUWB; bool enableUWB = true;
bool usingUWB; bool usingUWB = false;
bool onboardPosition; bool onboardPosition = false;
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
// Initialize the node and it's handles
ros::init(argc, argv, "LocalizationServer"); ros::init(argc, argv, "LocalizationServer");
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
//ros_namespace = ros::this_node::getNamespace(); // Needed? ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
// load context parameters // load context parameters
if(!loadParameters(nodeHandle)) if(!loadParameters(nodeHandle))
return -1; return -1;
// connect to the central manager service to receive CF Context and subscribe to CF Context updates of the DB
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);
// Bool if using UWB, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here // subscribe to the global vicon publisher and local uwb publisher
enableUWB = true;
usingUWB = false;
onboardPosition = false;
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback); ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to Vicon"); ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
//if (enableUWB){
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback); ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to UWB"); ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
//}
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1); //TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
ROS_INFO_STREAM("LocalizationServer started!"); // initialize the position data publisher
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
std::string package_path; std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/"; package_path = ros::package::getPath("d_fall_pps") + "/";
std::string record_file = package_path + "log/LocalizationServerLog.bag"; std::string record_file = package_path + "log/LocalizationServerLog.bag";
bag.open(record_file, rosbag::bagmode::Write); bag.open(record_file, rosbag::bagmode::Write);
ROS_INFO_STREAM("[LocalizationServer] started!");
ros::spin(); ros::spin();
bag.close(); bag.close();
...@@ -87,7 +82,7 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle) ...@@ -87,7 +82,7 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{ {
if(!nodeHandle.getParam("clientID", clientID)) if(!nodeHandle.getParam("clientID", clientID))
{ {
ROS_ERROR("Failed to get clientID!"); ROS_ERROR("[LocalizationServer] Failed to get clientID!");
return false; return false;
} }
...@@ -103,9 +98,8 @@ void d_fall_pps::loadCrazyflieContext() ...@@ -103,9 +98,8 @@ void d_fall_pps::loadCrazyflieContext()
if(centralManager.call(contextCall)) if(centralManager.call(contextCall))
{ {
context =contextCall.response.crazyflieContext; context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("[LocalizationServer] context updated");
ROS_INFO_STREAM("LocalizationServer context updated");
} }
} }
......
...@@ -16,14 +16,19 @@ ...@@ -16,14 +16,19 @@
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
//#include "ros/ros.h"
#include <ros/package.h>
#include <rosbag/bag.h>
#include "DataStreamClient.h" #include "DataStreamClient.h"
#include "ros/ros.h"
#include "d_fall_pps/CrazyflieContext.h" #include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h" #include "std_msgs/Float32.h"
#include "std_msgs/Float32MultiArray.h" #include "std_msgs/Float32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h" #include "d_fall_pps/CrazyflieData.h"
#include <rosbag/bag.h>
#include <ros/package.h>
#include "d_fall_pps/CMQuery.h" #include "d_fall_pps/CMQuery.h"
...@@ -44,59 +49,63 @@ ros::Publisher UWBDataPublisher; ...@@ -44,59 +49,63 @@ ros::Publisher UWBDataPublisher;
double rollPitchYaw[3]; double rollPitchYaw[3];
bool onboardPosition; bool onboardPosition = false;
bool useTiagoData; bool useTiagoData = true;
UWBAnchorArray anchors; UWBAnchorArray anchors;
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
ros::init(argc, argv, "UWBDataPublisher"); ros::init(argc, argv, "UWBDataPublisher");
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
ros::Time::init(); ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
//ros::Time::init();
if(!loadParameters(nodeHandle)) if(!loadParameters(nodeHandle))
return -1; return -1;
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);
// Bool if using UWB onboard position, tiagoData, TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
onboardPosition = false; // Subscribe to the anchor positions and all relevant Data from crazyradio
useTiagoData = true; //ros::Subscriber anchorPosSubscriber = namespaceNodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
ros::Subscriber anchorPosSubscriber = nodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1); ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback);
ros::Subscriber uwbLocationSubscriber;
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
ros::Subscriber cfRPYSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/RPY",10,cfRPYCallback); if(onboardPosition)
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
ros::Subscriber uwbLocationSubscriber; else
{
if(useTiagoData)
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
else
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
}
if (onboardPosition){ // TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
}else{
if (useTiagoData){
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
}else{
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
}
}
ros::Subscriber anchorPosSubscriber = namespaceNodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback); UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
std::string package_path; std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/"; package_path = ros::package::getPath("d_fall_pps") + "/";
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);
ros::spin(); ROS_INFO_STREAM("[UWBDataPublisher] started!");
ros::spin();
bag.close(); bag.close();
return 0; return 0;
} }
void d_fall_pps::dbChangedCallback(const std_msgs::Int32& msg)
{
d_fall_pps::loadCrazyflieContext();
}
//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)
{ {
...@@ -127,7 +136,7 @@ void d_fall_pps::cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ) ...@@ -127,7 +136,7 @@ void d_fall_pps::cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
//Callback function to handle distance information from CF //Callback function to handle distance information from CF
void d_fall_pps::cfDistancesCallback(const std_msgs::Float32MultiArray &distances) void d_fall_pps::cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
{ {
CrazyflieData data; CrazyflieData data;
double arrXYZ[3] = {0}; double arrXYZ[3] = {0};
...@@ -153,17 +162,12 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times) ...@@ -153,17 +162,12 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
CrazyflieData data; CrazyflieData data;
double arrXYZ[3] = {0}; double arrXYZ[3] = {0};
int len = times.layout.dim.size();
std::vector<float> distances; std::vector<float> distances;
for (int i = 0; i < len; i++) for (int i = 0; i < UWB_NUM_ANCHORS; i++)
{ distances.push_back(tiagosAwesomeFormula(times.data[i]));
distances[i] = tiagosAwesomeFormula(times.data[i]);
}
calculateXYZ(distances, len, arrXYZ); //TODO: Implement!!! calculateXYZ(distances, arrXYZ); //TODO: Implement!!!
data.crazyflieName = context.crazyflieName; data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0]; data.x = arrXYZ[0];
...@@ -179,7 +183,7 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times) ...@@ -179,7 +183,7 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
} }
//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::vector<float> &distances, int distLen, double(&xyz)[3]) void d_fall_pps::calculateXYZ(const std::vector<float> &distances, double(&xyz)[3])
{ {
//add algorithm from forces - paul //add algorithm from forces - paul
...@@ -198,9 +202,8 @@ void d_fall_pps::loadCrazyflieContext() ...@@ -198,9 +202,8 @@ void d_fall_pps::loadCrazyflieContext()
if(centralManager.call(contextCall)) if(centralManager.call(contextCall))
{ {
context =contextCall.response.crazyflieContext; context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("[UWBDataPublisher] context updated");
ROS_INFO_STREAM("LocalizationServer context updated");
} }
} }
...@@ -219,7 +222,8 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle) ...@@ -219,7 +222,8 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
// calculatse distances from passed microseconds // calculatse distances from passed microseconds
float d_fall_pps::tiagosAwesomeFormula(uint32_t time) float d_fall_pps::tiagosAwesomeFormula(uint32_t time)
{ {
return (time * 75.0 * (1.0 / (128.0 * 499.2))); return time * SPEED_OF_LIGHT / LOCO_DECK_FRQ;
//return (time * 75.0 * (1.0 / (128.0 * 499.2)));
} }
// Callback function to handle new anchor positions // Callback function to handle new anchor positions
......
#include "ros/ros.h"
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>
//using namespace d_fall_pps
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWB_BaseStation");
ROS_INFO("STARTED UWB BASE STATION NODE");
return 0;
}
\ 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