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