Commit 22da8c6f authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Merge branch 'getDataFromCF' into 'uwbmaster'

UWB Data in ROS

See merge request D-FaLL/PandS-System/D-FaLL-System!4
parents 60311a71 17822e72
{
"files.associations": {
"iterator": "cpp",
"string": "cpp",
"string_view": "cpp",
"vector": "cpp"
}
}
\ No newline at end of file
......@@ -8,7 +8,7 @@ Installation with the install script is the easiest. You will need:
- the ``d_fall_pps`` package compressed in a file called ``package.tar.gz``
- the rule files for the USB connection to the crazyradio, called ``99-crazyflie.rules`` and ``99-crazyradio.rules``
These files all need to be in the same directory. To run the installation, move to the containing directory and call it with
These files all need to be in the same directory. To run the installation, move to the containing directory (pps\ install) and call it with
```
./pps_install.sh <student id>
```
......@@ -27,7 +27,7 @@ Create a new catkin workspace and copy the ``d_fall_pps`` package into the ``src
Add a new line in the ``/etc/hosts`` file that links the teacher's IP with the domain name ``teacher`` and create a file called ``/etc/StudentID`` that contains the student id. Only write digits without any other symbols or whitespace characters.
- USB Crazyradio: <br />
To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` to the directory ``/etc/udev/rules.d``.
To set up the crazyradio USB dongle just copy the rule files ``99-crazyflie.rules`` and ``99-crazyradio.rules`` from directory ``pps\ install/`` to the directory ``/etc/udev/rules.d``.
You also have to install the library pyusb:
```
......@@ -43,10 +43,12 @@ sudo pip install pyusb
```
- Source scripts in ``.bashrc``: <br />
You need to source the following scripts in the ``.bashrc`` file:
- the ROS setup script: ``/opt/ros/kinetic/setup.bash``
- the workspace setup script: ``<catkin workspace>/devel/setup.bash``
- the student setup script: ``<catkin workspace>/src/d_fall_pps/launch/Config.sh``
Add following lines to the bottom of the file ``~/.bashrc`` (replace ``<catkin workspace>`` with correct directory)
```
source /opt/ros/kinetic/setup.bash
source <catkin workspace>/devel/setup.bash
source <catkin workspace>/src/d_fall_pps/launch/Config.sh
```
The workspace setup script will only appear after the first compilation of the catkin workspace.
......@@ -64,6 +66,9 @@ must be replaced with
export ROS_HOSTNAME=teacher
```
### IP-Addresses
Currently the teacher's IP is ``10.42.0.10`` and the student's IP are of the format ``10.42.0.xx``, where xx is an unused address.
### Installation of cfclient
The steps to install the crazyflie client are taken from [here](https://github.com/bitcraze/crazyflie-clients-python). To install the cfclient you need to install its dependencies:
```
......
......@@ -77,18 +77,23 @@ Then go to the _IPv4 Settings_ and choose **Manual** as the _Method_ and then ad
**Don't forget to choose the Vicon network manually if you choose to enable WiFi and Ethernet simultaneously as described below!**
<br>
### Vicon, teacher and students
During installation process is the IP address of the teacher set to 10.42.0.10. (This value is written to the /etc/hosts file such that this IP address is accessible through the keyword _teacher_) <br>
Have a look at `Config.sh` in `~/pps_ws/src/d_fall_pps/launch/`
<br><img src="./pics/setup_pics/configsh1.png" style="width: 500px;"/> <br>
<br><img src="./pics/setup_pics/configsh2.png" style="width: 500px;"/> <br>
Here you see, that the ROS Master URI is set to be the teacher. This means that _roscore_ runs only on the teacher's computer. Your own IP address (_ROS IP_) is also set and taken from your Ethernet settings as defined in the section _Setting up the Vicon network_.
<br>
### IP-Addresses
Currently the teacher's IP is ``10.42.0.10`` and the student's IP are of the format ``10.42.0.xx``, where xx is an unused address.
### Using WLAN and Vicon simultaneously
Without some adjustments it is not possible to use an Ethernet and a wireless network at the same time. Vicon is connected via cable and therefore wouldn't allow a connection to the Internet. Because it's tedious to always unplug the cable just to be able to look something up on Google, we provide an explanation on how to enable simultaneous usage of WLAN and Vicon. <br>
**But careful: If you enable this setting you have to choose the Vicon network manually. This is a common mistake! You will get an error if the cable is inserted but not manually chosen as shown in the picture below.**
<br><img src="./pics/setup_pics/chooseVicon.jpg" style="width: 500px;"/> <br>
#### Step by Step:
Ubuntu allows multiple connections by default, but sometimes, we need to specify which one to use. In your comments, you have mentioned that you use LAN for the Intranet and WiFi for the Internet.
Ubuntu allows multiple connections by default, but sometimes, we need to specify which one to use. Here we use LAN for the Intranet and WiFi for the Internet.
So, firstly search for Network Connections in the unity dash. Then, under the Ethernet section, click 'Add' button.
<br><img src="./pics/setup_pics/internetethernet1.png" style="width: 500px;"/> <br>
......
......@@ -133,6 +133,8 @@ add_message_files(
Setpoint.msg
CrazyflieEntry.msg
CrazyflieDB.msg
UWBAnchor.msg
UWBAnchorArray.msg
#----------------------------------------------------------------------
DebugMsg.msg
)
......@@ -235,6 +237,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(TeacherService src/TeacherService.cpp)
add_executable(LocalizationServer src/LocalizationServer.cpp)
add_executable(PPSClient src/PPSClient.cpp)
......@@ -287,6 +290,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(TeacherService 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})
......@@ -326,6 +330,8 @@ target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
target_link_libraries(LocalizationServer ${catkin_LIBRARIES})
target_link_libraries(TeacherService ${catkin_LIBRARIES})
target_link_libraries(UWBDataPublisher ${catkin_LIBRARIES})
target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(Client ${catkin_LIBRARIES})
......
......@@ -32,6 +32,8 @@ import rosbag
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")
......@@ -153,23 +155,56 @@ class PPSRadioClient:
self._cf.close_link()
self.change_status_to(DISCONNECTED)
def _data_received_callback(self, timestamp, data, logconf):
def _anchorTime_received_callback(self, timestamp, data, anchorTime):
times = UInt32MultiArray()
times.data = [data["UWB_Anchor_Pos.anchor0"], data["UWB_Anchor_Pos.anchor1"]]
#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)))
bag.write('anchorTime', times)
uwb_location_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"]]
bag.write('anchorDistances',distances)
uwb_location_pub.publish(distances)
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
if onboardPosition:
arrXYZ = Float32MultiArray()
#TODO: where are coordinates published?
arrXYZ.data = [data["xxx.x"], fdata["xxx.y"],data["xxx.z"]]
bag.write('XYZ',arrXYZ)
uwb_location_pub.publish(arrXYZ)
arrRPY = Float32MultiArray()
arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
bag.write('rollPitchYaw',arrRPY)
rpy_pub.publish(arrRPY)
#print arrRPY
#def _data_received_callback(self, timestamp, data, logconf):
def _vbat_received_callback(self, timestamp, data, batLog):
#print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data)
batteryVolt = Float32()
stabilizerYaw = Float32()
stabilizerPitch = Float32()
stabilizerRoll = Float32()
batteryVolt.data = data["pm.vbat"]
stabilizerYaw.data = data["stabilizer.yaw"]
stabilizerPitch.data = data["stabilizer.pitch"]
bag.write('batteryVoltage', batteryVolt)
bag.write('stabilizerYaw', stabilizerYaw)
bag.write('stabilizerPitch', stabilizerPitch)
bag.write('stabilizerRoll', stabilizerRoll)
#publish battery voltage for GUI
#cfbattery_pub.publish(std_msgs.Float32(batteryVolt.data))
# print "batteryVolt: %s" % batteryVolt
#print "batteryVolt: %s" % batteryVolt
cfbattery_pub.publish(batteryVolt)
......@@ -179,23 +214,131 @@ class PPSRadioClient:
# def _init_logging(self):
def _start_logging(self):
self.logconf = LogConfig("LoggingTest", 100) # second variable is freq in ms
self.logconf.add_variable("stabilizer.roll", "float");
self.logconf.add_variable("stabilizer.pitch", "float");
self.logconf.add_variable("stabilizer.yaw", "float");
self.logconf.add_variable("pm.vbat", "float");
self._cf.log.add_config(self.logconf)
if self.logconf.valid:
self.logconf.data_received_cb.add_callback(self._data_received_callback)
self.logconf.error_cb.add_callback(self._logging_error)
print "logconf valid"
def init_log_battery(self):
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
self.batLog = LogConfig("BatteryLog", battery_polling_period) # second variable is period in ms
self.batLog.add_variable("pm.vbat", "float");
self._cf.log.add_config(self.batLog)
if self.batLog.valid:
self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
self.batLog.error_cb.add_callback(self._logging_error)
print "batLog valid"
else:
print "batLog invalid"
self.batLog.start()
print "batLog start"
def init_log_xyzrpy(self):
global xyzrpy_polling_period
xyzrpy_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/xyzrpy_polling_period")
self.xyzrpy = LogConfig("XYZRPYLog", xyzrpy_polling_period)
#TODO: where are coordinates published?
if onboardPosition:
self.xyzrpy.add_variable("xxx.x", "float");
self.xyzrpy.add_variable("xxx.y", "float");
self.xyzrpy.add_variable("xxx.z", "float")
self.xyzrpy.add_variable("stabilizer.roll", "float");
self.xyzrpy.add_variable("stabilizer.pitch", "float");
self.xyzrpy.add_variable("stabilizer.yaw", "float");
self._cf.log.add_config(self.xyzrpy)
if self.xyzrpy.valid:
self.xyzrpy.data_received_cb.add_callback(self._xyzrpy_received_callback)
self.xyzrpy.error_cb.add_callback(self._logging_error)
print "xyzrpy valid"
else:
print "xyzrpy invalid"
self.xyzrpy.start()
print "xyzrpy start"
def init_log_anchordist(self):
global anchordist_polling_period
anchordist_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/anchordist_polling_period")
self.anchorDist = LogConfig("AnchorDistances", anchordist_polling_period)
self.anchorDist.add_variable("ranging.distance1","float");
self.anchorDist.add_variable("ranging.distance2","float");
self.anchorDist.add_variable("ranging.distance3","float");
self.anchorDist.add_variable("ranging.distance4","float");
self.anchorDist.add_variable("ranging.distance5","float");
self.anchorDist.add_variable("ranging.distance6","float");
self._cf.log.add_config(self.anchorDist)
if self.anchorDist.valid:
self.anchorDist.data_received_cb.add_callback(self._anchorDist_received_callback)
self.anchorDist.error_cb.add_callback(self._logging_error)
print "anchorDist valid"
else:
print "logconf invalid"
print "anchorDist invalid"
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):
self.logconf.start()
print "logconf start"
self.init_log_battery()
if enableUWB:
self.init_log_xyzrpy()
if not onboardPosition:
if useTiagoData:
self.init_log_tiago()
else:
self.init_log_anchordist()
def _stop_logging(self):
self.batLog.stop()
rospy.loginfo("batLog stopped")
self.batLog.delete()
rospy.loginfo("batLog deleted")
if enableUWB:
self.xyzrpy.stop()
rospy.loginfo("xyzrpy stopped")
self.xyzrpy.delete()
rospy.loginfo("xyzrpy deleted")
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")
def _connected(self, link_uri):
"""
......@@ -210,9 +353,6 @@ class PPSRadioClient:
# Config for Logging
self._start_logging()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
......@@ -233,10 +373,7 @@ class PPSRadioClient:
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
self.logconf.stop()
rospy.loginfo("logconf stopped")
self.logconf.delete()
rospy.loginfo("logconf deleted")
self._stop_logging()
def _send_to_commander_motor(self, cmd1, cmd2, cmd3, cmd4):
pk = CRTPPacket()
......@@ -305,6 +442,9 @@ if __name__ == '__main__':
global node_name
node_name = "CrazyRadio"
rospy.init_node(node_name, anonymous=True)
global ros_namespace
ros_namespace = rospy.get_namespace()
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
......@@ -316,9 +456,35 @@ if __name__ == '__main__':
# radio_address = "radio://0/72/2M"
# rospy.loginfo("manual address loaded")
# 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
global enableUWB
enableUWB = True
global useUWB
useUWB = False
global onboardPosition #Not yet working...
onboardPosition = False #Not yet working...
global useTiagoData
useTiagoData = True
global cfbattery_pub
cfbattery_pub = rospy.Publisher(node_name + '/CFBattery', Float32, queue_size=10)
if enableUWB:
global rpy_pub
rpy_pub = rospy.Publisher(node_name + '/RPY', Float32MultiArray, queue_size=10)
global uwb_location_pub
if onboardPosition:
uwb_location_pub = rospy.Publisher(node_name + '/XYZ', Float32MultiArray, queue_size=10) #Not yet working...
else:
if useTiagoData:
uwb_location_pub = rospy.Publisher(node_name + '/times', UInt32MultiArray, queue_size=10)
else:
uwb_location_pub = rospy.Publisher(node_name + '/distances', Float32MultiArray, queue_size=10)
global cf_client
cf_client = PPSRadioClient()
......
......@@ -17,6 +17,7 @@
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
namespace d_fall_pps
{
......@@ -31,6 +32,9 @@ namespace d_fall_pps
// Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32 &msg);
// Callback function to handle incoming UWB data
void UWBDataCallback(const CrazyflieData &data);
}
#endif // LOCALIZATIONSERVER_H included
\ No newline at end of file
// ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// 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 "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"
namespace d_fall_pps
{
//Callback function to handle roll pitch yaw information from CF
void cfRPYCallback(const std_msgs::Float32MultiArray &arrRPY);
//Callback funtion to hande x y z information from CF
void cfXYZCallback(const std_msgs::Float32MultiArray &arrXYZ);
//Callback function to handle distance information from CF
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 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
......@@ -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>
......
uint32 id
float32 x
float32 y
float32 z
\ No newline at end of file
uint32 length
UWBAnchor[] data
\ No newline at end of file
......@@ -2,6 +2,8 @@ safeController: "SafeControllerService/RateController"
customController: "CustomControllerService/CustomController"
strictSafety: true
angleMargin: 0.6
battery_threshold_while_flying: 2.9
battery_threshold_while_motors_off: 3.3
battery_threshold_while_flying: 2.9 # in V
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 500 # in ms
xyzrpy_polling_period: 50
anchordist_polling_period: 50
......@@ -27,9 +27,6 @@ using namespace d_fall_pps;
// the clientID defines namespace and identifier in the CentralManagerService
int clientID;
// values for safetyCheck
bool strictSafety;
// Self explanatory variables
ros::ServiceClient centralManager;
ros::Publisher localizationPublisher;
......@@ -37,6 +34,10 @@ ros::Publisher localizationPublisher;
CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB;
bool usingUWB;
bool onboardPosition;
int main(int argc, char* argv[])
{
ros::init(argc, argv, "LocalizationServer");
......@@ -50,12 +51,23 @@ int main(int argc, char* argv[])
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
// 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;
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to Vicon");
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
//if (enableUWB){
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData",100,UWBDataCallback);
ROS_INFO_STREAM("LocalizationServer connected to UWB");
//}
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);
ROS_INFO_STREAM("LocalizationServer started!");
......@@ -114,6 +126,13 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
}
}
// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
localizationPublisher.publish(data);
bag.write("UWBData", ros::Time::now(), data);
}
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
d_fall_pps::loadCrazyflieContext();
......
// ROS node that publishes the localization data from sensor fusion of the crazyflie, using uwb for coordinates
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// 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"
#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"