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
CMD_CRAZYFLY_LAND = 4
CMD_CRAZYFLY_MOTORS_OFF = 5
# max Number of UWB Anchors
UWB_NUM_ANCHORS = 6
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf')
......@@ -156,15 +159,18 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
def _anchorTime_received_callback(self, timestamp, data, anchorTime):
times = UInt32MultiArray()
times.data = [data["UWB_Anchor_Pos.anchor0"], data["UWB_Anchor_Pos.anchor1"]]
ticks = UInt32MultiArray()
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...
print "dist0: %s" % (times.data[0] )#* 75.0 * (1.0 / (128.0 * 499.2)))
print "dist1: %s" % (times.data[1] )#* 75.0 * (1.0 / (128.0 * 499.2)))
# for debugging only
#for i in range(UWB_NUM_ANCHORS):
# rospy.loginfo("Anchor " + str(i) + ": " + str(ticks.data[i]))
bag.write('anchorTime', times)
uwb_location_pub.publish(times)
bag.write('anchorTime', ticks)
uwb_location_pub.publish(ticks)
def _anchorDist_received_callback(self, timestamp, data, anchorDist):
distances = Float32MultiArray()
......@@ -287,8 +293,12 @@ class PPSRadioClient:
global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/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)
if self.anchorTime.valid:
......
......@@ -14,18 +14,29 @@
// You should have received a copy of the GNU General Public License
// 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/Float32MultiArray.h"
#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"
#define UWB_NUM_ANCHORS 6
#define SPEED_OF_LIGHT 299792458.0
#define LOCO_DECK_FRQ (499.2e6 * 128)
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
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
......
......@@ -11,9 +11,6 @@
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</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>
......
......@@ -30,53 +30,48 @@ int clientID;
// Self explanatory variables
ros::ServiceClient centralManager;
ros::Publisher localizationPublisher;
//std::string ros_namespace; // Needed?
CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB;
bool usingUWB;
bool onboardPosition;
bool enableUWB = true;
bool usingUWB = false;
bool onboardPosition = false;
int main(int argc, char* argv[])
{
// Initialize the node and it's handles
ros::init(argc, argv, "LocalizationServer");
ros::NodeHandle nodeHandle("~");
//ros_namespace = ros::this_node::getNamespace(); // Needed?
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
// load context parameters
if(!loadParameters(nodeHandle))
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);
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
enableUWB = true;
usingUWB = false;
onboardPosition = false;
// subscribe to the global vicon publisher and local uwb publisher
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to Vicon");
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
//if (enableUWB){
ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
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;
package_path = ros::package::getPath("d_fall_pps") + "/";
std::string record_file = package_path + "log/LocalizationServerLog.bag";
bag.open(record_file, rosbag::bagmode::Write);
ROS_INFO_STREAM("[LocalizationServer] started!");
ros::spin();
bag.close();
......@@ -87,7 +82,7 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
if(!nodeHandle.getParam("clientID", clientID))
{
ROS_ERROR("Failed to get clientID!");
ROS_ERROR("[LocalizationServer] Failed to get clientID!");
return false;
}
......@@ -103,9 +98,8 @@ void d_fall_pps::loadCrazyflieContext()
if(centralManager.call(contextCall))
{
context =contextCall.response.crazyflieContext;
ROS_INFO_STREAM("LocalizationServer context updated");
context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("[LocalizationServer] context updated");
}
}
......
......@@ -16,14 +16,19 @@
#include <stdlib.h>
#include <string.h>
//#include "ros/ros.h"
#include <ros/package.h>
#include <rosbag/bag.h>
#include "DataStreamClient.h"
#include "ros/ros.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Float32MultiArray.h"
#include "d_fall_pps/CrazyflieData.h"
#include <rosbag/bag.h>
#include <ros/package.h>
#include "d_fall_pps/CMQuery.h"
......@@ -44,59 +49,63 @@ ros::Publisher UWBDataPublisher;
double rollPitchYaw[3];
bool onboardPosition;
bool useTiagoData;
bool onboardPosition = false;
bool useTiagoData = true;
UWBAnchorArray anchors;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "UWBDataPublisher");
ros::NodeHandle nodeHandle("~");
ros::Time::init();
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
//ros::Time::init();
if(!loadParameters(nodeHandle))
return -1;
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
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;
useTiagoData = true;
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
// 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;
if (onboardPosition){
if(onboardPosition)
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
}else{
if (useTiagoData){
else
{
if(useTiagoData)
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
}else{
else
uwbLocationSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
}
}
ros::Subscriber anchorPosSubscriber = namespaceNodeHandle.subscribe("/TeacherService/anchorPos",1,anchorPosCallback);
// TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
UWBDataPublisher = nodeHandle.advertise<CrazyflieData>("UWBData", 1);
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
std::string record_file = package_path + "log/UWBDataPublisherLog.bag";
bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
ROS_INFO_STREAM("[UWBDataPublisher] started!");
ros::spin();
bag.close();
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
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
......@@ -153,17 +162,12 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
CrazyflieData data;
double arrXYZ[3] = {0};
int len = times.layout.dim.size();
std::vector<float> distances;
for (int i = 0; i < len; i++)
{
distances[i] = tiagosAwesomeFormula(times.data[i]);
}
for (int i = 0; i < UWB_NUM_ANCHORS; i++)
distances.push_back(tiagosAwesomeFormula(times.data[i]));
calculateXYZ(distances, len, arrXYZ); //TODO: Implement!!!
calculateXYZ(distances, arrXYZ); //TODO: Implement!!!
data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0];
......@@ -179,7 +183,7 @@ void d_fall_pps::cfTimesCallback(const std_msgs::UInt32MultiArray &times)
}
//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
......@@ -198,9 +202,8 @@ void d_fall_pps::loadCrazyflieContext()
if(centralManager.call(contextCall))
{
context =contextCall.response.crazyflieContext;
ROS_INFO_STREAM("LocalizationServer context updated");
context = contextCall.response.crazyflieContext;
ROS_INFO_STREAM("[UWBDataPublisher] context updated");
}
}
......@@ -219,7 +222,8 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
// calculatse distances from passed microseconds
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
......
#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