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

fixed errors in UWBDataPublisher, added better handling of new data formats from crazyflie

parent 16ac4f00
......@@ -234,6 +234,7 @@ include_directories(
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
add_executable(UWBDataPublisher src/UWBDataPublisher.cpp)
add_executable(LocalizationServer src/LocalizationServer.cpp)
add_executable(PPSClient src/PPSClient.cpp)
add_executable(Client src/Client.cpp)
......@@ -285,6 +286,7 @@ qt5_use_modules(student_GUI Widgets)
add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(LocalizationServer d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(UWBDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(Client d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -322,6 +324,8 @@ target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
target_link_libraries(LocalizationServer ${catkin_LIBRARIES})
target_link_libraries(UWBDataPublisher ${catkin_LIBRARIES})
target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(Client ${catkin_LIBRARIES})
......
......@@ -33,6 +33,7 @@ from rospkg import RosPack
from std_msgs.msg import Float32
from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import UInt32MultiArray
# Add library
#sys.path.append("lib")
......@@ -154,6 +155,16 @@ class PPSRadioClient:
self._cf.close_link()
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"]]
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)))
bag.write('anchorTime', times)
times_pub.publish(times)
def _anchorDist_received_callback(self, timestamp, data, anchorDist):
distances = Float32MultiArray()
distances.data = [data["ranging.distance1"],data["ranging.distance2"],data["ranging.distance3"],data["ranging.distance4"],data["ranging.distance5"],data["ranging.distance6"]]
......@@ -163,18 +174,18 @@ class PPSRadioClient:
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
#if onboardPosition:
#arrXYZ = Float32MultiArray()
#arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
#bag.write('XYZ',arrXYZ)
#xyz_pub.publish(arrXYZ)
if onboardPosition:
arrXYZ = Float32MultiArray()
arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
bag.write('XYZ',arrXYZ)
xyz_pub.publish(arrXYZ)
#print "anchor0: %s" % data["UWB_Anchor_Pos.anchor0"]
#print "anchor1: %s" % data["UWB_Anchor_Pos.anchor1"]
dist0 = data["UWB_Anchor_Pos.anchor0"] * 75.0 * (1.0 / (128.0 * 499.2))
print "dist0: %s" % dist0
dist1 = data["UWB_Anchor_Pos.anchor1"] * 75.0 * (1.0 / (128.0 * 499.2))
print "dist1: %s" % dist1
#dist0 = data["UWB_Anchor_Pos.anchor0"] * 75.0 * (1.0 / (128.0 * 499.2))
#print "dist0: %s" % dist0
#dist1 = data["UWB_Anchor_Pos.anchor1"] * 75.0 * (1.0 / (128.0 * 499.2))
#print "dist1: %s" % dist1
arrRPY = Float32MultiArray()
......@@ -234,13 +245,13 @@ class PPSRadioClient:
self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
#TODO: where are coordinates published?
#if onboardPosition:
#self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
#self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
#self.xyzrpy.add_variable("xxx.z", "float")
if onboardPosition:
self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
self.xyzrpy.add_variable("xxx.z", "float")
#self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor0", "uint32_t");
#self.xyzrpy.add_variable("UWB_Anchor_Pos.anchor1", "uint32_t");
self.xyzrpy.add_variable("stabilizer.roll", "float");
self.xyzrpy.add_variable("stabilizer.pitch", "float");
......@@ -280,6 +291,25 @@ class PPSRadioClient:
self.anchorDist.start()
print "anchorDist start"
def init_log_tiago(self):
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");
self._cf.log.add_config(self.anchorTime)
if self.anchorTime.valid:
self.anchorTime.data_received_cb.add_callback(self._anchorTime_received_callback)
self.anchorTime.error_cb.add_callback(self._logging_error)
print "anchorTime valid"
else:
print "anchorTime invalid"
self.anchorTime.start()
print "anchorTime start"
def _start_logging(self):
......@@ -287,8 +317,11 @@ class PPSRadioClient:
if enableUWB:
self.init_log_xyzrpy()
#if not onboardPosition:
#self.init_log_anchordist() #TODO: test
if not onboardPosition:
if useTiagoData:
self.init_log_tiago()
else:
self.init_log_anchordist() #TODO: test
def _stop_logging(self):
......@@ -303,11 +336,17 @@ class PPSRadioClient:
self.xyzrpy.delete()
rospy.loginfo("xyzrpy deleted")
'''if not onboardPosition:
if not onboardPosition:
if useTiagoData:
self.anchorTime.stop()
rospy.loginfo("anchorTime stopped")
self.anchorTime.delete()
rospy.loginfo("anchorTime deleted")
else:
self.anchorDist.stop()
rospy.loginfo("anchorDist stopped")
self.anchorDist.delete()
rospy.loginfo("anchorDist deleted")'''
rospy.loginfo("anchorDist deleted")
def _connected(self, link_uri):
"""
......@@ -432,6 +471,8 @@ if __name__ == '__main__':
useUWB = False
global onboardPosition
onboardPosition = False
global useTiagoData
useTiagoData = True
global cfbattery_pub
......@@ -448,6 +489,10 @@ if __name__ == '__main__':
xyz_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10)
#TODO: To publish distances, uncomment these lines, uncomment self.init_log_anchordist() in _start_logging, uncomment stop functions in _stop_logging
else:
if useTiagoData:
global times_pub
times_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
else:
global distances_pub
distances_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
......
......@@ -16,23 +16,35 @@
#include "std_msgs/Int32.h"
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/UInt32MultiArray.h"
#include <vector>
#include <stdint.h>
#include "ros/ros.h"
namespace d_fall_pps
{
//Callback function to handle roll pitch yaw information from CF
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
//Callback funtion to hande x y z information from CF
void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ);
//Callback function to handle distance information from CF
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances);
// Loads the current context of the client
void loadCrazyflieContext();
// Loads the parameters of the node handle
bool loadParameters(ros::NodeHandle &nodeHandle);
// 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
void cfTimesCallback(const std_msgs::UInt32MultiArray &times);
// calculatse distances from passed microseconds
float tiagosAwesomeFormula(uint32_t time);
}
\ No newline at end of file
......@@ -13,6 +13,10 @@
<param name="clientID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="UWBDataPublisher" output="screen" type="UWBDataPublisher">
<param name="clientID" value="$(optenv ROS_NAMESPACE)" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
......
......@@ -14,6 +14,7 @@
// 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 <stdlib.h>
#include <string.h>
#include "DataStreamClient.h"
#include "ros/ros.h"
......@@ -21,10 +22,12 @@
#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"
#include "UWBDataPublisher"
#include "UWBDataPublisher.h"
using namespace d_fall_pps;
......@@ -34,11 +37,13 @@ int clientID;
CrazyflieContext context;
rosbag::Bag bag;
ros::ServiceClient centralManager;
ros::Publisher UWBDataPublisher;
double[3] rollPitchYaw;
double rollPitchYaw[3];
bool onboardPosition;
bool useTiagoData;
int main(int argc, char* argv[])
{
......@@ -50,10 +55,12 @@ int main(int argc, char* argv[])
if(!loadParameters(nodeHandle))
return -1;
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
// Bool if using UWB onboard position, 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);
......@@ -62,10 +69,17 @@ int main(int argc, char* argv[])
if (onboardPosition){
ros::Subscriber cfXYZSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/XYZ",10,cfXYZCallback);
}else{
if (useTiagoData){
ros::Subscriber cfTimesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/times",10,cfTimesCallback);
}else{
ros::Subscriber cfDistancesSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/distances",10,cfDistancesCallback);
}
}
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();
......@@ -78,11 +92,13 @@ int main(int argc, char* argv[])
//Callback function to handle roll pitch yaw information from CF
void d_fall_pps::cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY)
{
rollPitchYaw = (double)arrRPY.data;
rollPitchYaw[0] = arrRPY.data[0];
rollPitchYaw[1] = arrRPY.data[1];
rollPitchYaw[2] = arrRPY.data[2];
}
//Callback funtion to hande x y z information from CF
void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
void d_fall_pps::cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
{
CrazyflieData data;
......@@ -101,13 +117,13 @@ void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ)
}
//Callback function to handle distance information from CF
void cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
void d_fall_pps::cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
{
CrazyflieData data;
double[3] arrXYZ = [0,0,0];
double arrXYZ[3] = {0};
calculateXYZ((double)distances.data, arrXYZ); //TODO: Implement!!!
calculateXYZ(distances.data, distances.layout.dim.size(), arrXYZ); //TODO: Implement!!!
data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0];
......@@ -123,8 +139,39 @@ void cfDistancesCallback(const std_msgs::Float32MultiArray &distances)
}
// Callback functions to handle times information from CF
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]);
}
calculateXYZ(distances, len, arrXYZ); //TODO: Implement!!!
data.crazyflieName = context.crazyflieName;
data.x = arrXYZ[0];
data.y = arrXYZ[1];
data.z = arrXYZ[2];
data.roll = rollPitchYaw[0];
data.pitch = rollPitchYaw[1];
data.yaw = rollPitchYaw[2];
data.occluded = false;
data.acquiringTime = 0; //???
UWBDataPublisher.publish(data);
}
//calculates position from anchor locations and distances -> forces algorithm?
void calculateXYZ(const double[] &distances, double(&xyz)[3])
void d_fall_pps::calculateXYZ(const std::vector<float> &distances, int distLen, double(&xyz)[3])
{
//add algorithm from forces - paul
......@@ -158,3 +205,9 @@ bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
return true;
}
// calculatse distances from passed microseconds
float d_fall_pps::tiagosAwesomeFormula(uint32_t time)
{
return (time * 75.0 * (1.0 / (128.0 * 499.2)));
}
\ 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