Commit 713cbe73 authored by tiagos's avatar tiagos
Browse files

Merge branch 'uwbmaster' of https://gitlab.ethz.ch/D-FaLL/PandS-System/D-FaLL-System into uwbmaster

parents e82a6b2b e3218075
pps_ws/build/
pps_ws/devel/
.vscode/
*.pyc
*.orig
......
......@@ -81,7 +81,7 @@ public:
~MainWindow();
private slots:
void updateNewViconData(const ptrToMessage& p_msg);
void updatePositionData(const CrazyflieData& data);
void on_RF_Connect_button_clicked();
void on_take_off_button_clicked();
......
......@@ -47,8 +47,7 @@ public:
bool init();
// void messageCallback(const ViconData& data);
void messageCallback(const ptrToMessage& p_msg);
void localizationCallback(const CrazyflieData& data);
ros::ServiceClient m_read_db_client;
ros::ServiceClient m_update_db_client;
......@@ -57,7 +56,7 @@ public:
signals:
void newViconData(const ptrToMessage& p_msg);
void newPositionData(const CrazyflieData& data);
public slots:
void run();
......@@ -69,9 +68,7 @@ private:
QThread * m_pThread;
ViconData m_vicon_data;
ros::Subscriber m_vicon_subscriber;
ros::Subscriber m_localization_subscriber;
// ros::Publisher sim_velocity;
};
#endif
......
......@@ -43,8 +43,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
m_ros_namespace = ros::this_node::getNamespace();
ROS_INFO("namespace: %s", m_ros_namespace.c_str());
qRegisterMetaType<ptrToMessage>("ptrToMessage");
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
qRegisterMetaType<CrazyflieData>("CrazyflieData");
QObject::connect(m_rosNodeThread, SIGNAL(newPositionData(const CrazyflieData&)), this, SLOT(updatePositionData(const CrazyflieData&)));
ros::NodeHandle nodeHandle(m_ros_namespace);
......@@ -299,13 +299,13 @@ void MainWindow::updateBatteryVoltage(float battery_voltage)
QString qstr = "";
qstr.append(QString::number(battery_voltage, 'f', 2));
qstr.append(" V");
ROS_INFO_STREAM("battery voltage " << battery_voltage);
//ROS_INFO_STREAM("battery voltage " << battery_voltage);
ui->voltage_field->setText(qstr);
}
void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
{
ROS_INFO("callback CFBattery received in GUI");
//ROS_INFO("callback CFBattery received in GUI");
updateBatteryVoltage(msg.data);
}
......@@ -358,44 +358,36 @@ void MainWindow::coordinatesToLocal(CrazyflieData& cf)
cf.z -= originZ;
}
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
{
CrazyflieData global = *it;
if(global.crazyflieName == m_context.crazyflieName)
{
CrazyflieData local = global;
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
ui->current_x_2->setText(QString::number(local.x, 'f', 3));
ui->current_y_2->setText(QString::number(local.y, 'f', 3));
ui->current_z_2->setText(QString::number(local.z, 'f', 3));
ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff
ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
}
void MainWindow::updatePositionData(const CrazyflieData& data)
{
CrazyflieData local = data;
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
ui->current_x->setText(QString::number(local.x, 'f', 3));
ui->current_y->setText(QString::number(local.y, 'f', 3));
ui->current_z->setText(QString::number(local.z, 'f', 3));
ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
ui->current_x_2->setText(QString::number(local.x, 'f', 3));
ui->current_y_2->setText(QString::number(local.y, 'f', 3));
ui->current_z_2->setText(QString::number(local.z, 'f', 3));
ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
// also update diff
ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
void MainWindow::on_RF_Connect_button_clicked()
......
......@@ -59,8 +59,10 @@ bool rosNodeThread::init()
ros::start();
ros::Time::init();
ros::NodeHandle nh("~");
ros::NodeHandle namespaceNodehandle = ros::NodeHandle();
m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
//m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
m_localization_subscriber = namespaceNodehandle.subscribe("LocalizationServer/LocalizationData", 100, &rosNodeThread::localizationCallback, this);
// clients for db services:
m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
......@@ -71,9 +73,9 @@ bool rosNodeThread::init()
return true;
} // set up the thread
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
void rosNodeThread::localizationCallback(const CrazyflieData& data)
{
emit newViconData(p_msg); //pass the message to other places
emit newPositionData(data);
}
void rosNodeThread::run()
......
......@@ -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')
......@@ -125,76 +128,57 @@ class PPSRadioClient:
self.change_status_to(DISCONNECTED)
def change_status_to(self, new_status):
print "status changed to: %s" % new_status
self._status = new_status
self.status_pub.publish(new_status)
def get_status(self):
return self._status
def update_link_uri(self):
self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
def connect(self):
# update link from ros params
self.update_link_uri()
print "Connecting to %s" % self.link_uri
self.change_status_to(CONNECTING)
rospy.loginfo("connecting...")
self._cf.open_link(self.link_uri)
"""
---------------------------------------
def disconnect(self):
print "Motors OFF"
self._send_to_commander_motor(0, 0, 0, 0)
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
time.sleep(0.1)
print "Disconnecting from %s" % self.link_uri
self._cf.close_link()
self.change_status_to(DISCONNECTED)
Logging functions
def _anchorTime_received_callback(self, timestamp, data, anchorTime):
times = UInt32MultiArray()
times.data = [data["UWB_Anchor_Pos.anchor0"], data["UWB_Anchor_Pos.anchor1"]]
---------------------------------------
"""
def _start_logging(self):
#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)))
self.init_log_battery()
bag.write('anchorTime', times)
uwb_location_pub.publish(times)
if enableUWB:
self.init_log_uwb()
self.init_log_rpy()
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"]]
def _stop_logging(self):
bag.write('anchorDistances',distances)
uwb_location_pub.publish(distances)
self.stop_log_battery()
def _xyzrpy_received_callback(self, timestamp, data, xyzrpy):
if enableUWB:
self.stop_log_uwb()
self.stop_log_rpy()
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)
def _logging_error(self, logconf, msg):
rospy.logerr("[CrazyRadio] Error when logging %s", logconf.name)
arrRPY = Float32MultiArray()
arrRPY.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
### Battery log
def init_log_battery(self):
bag.write('rollPitchYaw',arrRPY)
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
rpy_pub.publish(arrRPY)
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)
#print arrRPY
if self.batLog.valid:
self.batLog.data_received_cb.add_callback(self._vbat_received_callback)
self.batLog.error_cb.add_callback(self._logging_error)
rospy.loginfo("[CrazyRadio] batLog valid")
else:
rospy.logwarn("[CrazyRadio] batLog invalid")
self.batLog.start()
rospy.loginfo("[CrazyRadio] batLog started")
#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()
......@@ -207,139 +191,105 @@ class PPSRadioClient:
#print "batteryVolt: %s" % batteryVolt
cfbattery_pub.publish(batteryVolt)
def stop_log_battery(self):
self.batLog.stop()
rospy.loginfo("[CrazyRadio] batLog stopped")
self.batLog.delete()
rospy.loginfo("[CrazyRadio] batLog deleted")
def _logging_error(self, logconf, msg):
print "Error when logging %s" % logconf.name
### UWB log
def init_log_uwb(self):
# def _init_logging(self):
polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
self.anchorLog = LogConfig("AnchorDistances", polling_period)
def init_log_battery(self):
for i in range(UWB_NUM_ANCHORS):
self.anchorLog.add_variable("UWB_Anchor_Pos.anchor" + str(i), "uint32_t");
global battery_polling_period
battery_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/battery_polling_period")
self._cf.log.add_config(self.anchorLog)
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"
if self.anchorLog.valid:
self.anchorLog.data_received_cb.add_callback(self._anchors_received_callback)
self.anchorLog.error_cb.add_callback(self._logging_error)
rospy.loginfo("[CrazyRadio] anchorLog valid")
else:
print "batLog invalid"
rospy.logwarn("[CrazyRadio] anchorLog invalid")
self.batLog.start()
print "batLog start"
self.anchorLog.start()
rospy.loginfo("[CrazyRadio] anchorLog started")
def init_log_xyzrpy(self):
def _anchors_received_callback(self, timestamp, data, anchorDist):
global xyzrpy_polling_period
xyzrpy_polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/xyzrpy_polling_period")
dist = UInt32MultiArray()
dist.data = []
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")
for i in range(UWB_NUM_ANCHORS):
dist.data.insert(i, data["UWB_Anchor_Pos.anchor" + str(i)])
self.xyzrpy.add_variable("stabilizer.roll", "float");
self.xyzrpy.add_variable("stabilizer.pitch", "float");
self.xyzrpy.add_variable("stabilizer.yaw", "float");
uwb_location_pub.publish(dist)
#bag.write('anchorTime', dist) # needs a lot of time
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 "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"
# for debugging
#for i in range(UWB_NUM_ANCHORS):
# rospy.loginfo("Anchor " + str(i) + ": " + str(dist.data[i]))
def stop_log_uwb(self):
self.anchorLog.stop()
rospy.loginfo("[CrazyRadio] anchorLog stopped")
self.anchorLog.delete()
rospy.loginfo("[CrazyRadio] anchorLog deleted")
### RPY log
def init_log_rpy(self):
polling_period = rospy.get_param(ros_namespace + "/CrazyRadio/positioning_polling_period")
self.rpyLog = LogConfig("RPY", polling_period)
self.rpyLog.add_variable("stabilizer.roll", "float");
self.rpyLog.add_variable("stabilizer.pitch", "float");
self.rpyLog.add_variable("stabilizer.yaw", "float");
self._cf.log.add_config(self.rpyLog)
if self.rpyLog.valid:
self.rpyLog.data_received_cb.add_callback(self._rpy_received_callback)
self.rpyLog.error_cb.add_callback(self._logging_error)
rospy.loginfo("[CrazyRadio] rpyLog valid")
else:
print "anchorTime invalid"
rospy.logwarn("[CrazyRadio] rpyLog invalid")
self.anchorTime.start()
print "anchorTime start"
self.rpyLog.start()
rospy.loginfo("[CrazyRadio] rpyLog started")
def _rpy_received_callback(self, timestamp, data, rpy):
rpy_data = Float32MultiArray()
rpy_data.data = [data["stabilizer.roll"],data["stabilizer.pitch"],data["stabilizer.yaw"]]
rpy_pub.publish(rpy_data)
#bag.write('RPY', rpy_data) # needs a lot of time
def _start_logging(self):
# for debugging
#rospy.loginfo("Roll: %f\nPitch: %f\nYaw: %f", rpy_data.data[0], rpy_data.data[1], rpy_data.data[2])
self.init_log_battery()
def stop_log_rpy(self):
if enableUWB:
self.init_log_xyzrpy()
if not onboardPosition:
if useTiagoData:
self.init_log_tiago()
else:
self.init_log_anchordist()
self.rpyLog.stop()
rospy.loginfo("[CrazyRadio] rpyLog stopped")
self.rpyLog.delete()
rospy.loginfo("[CrazyRadio] rpyLog deleted")
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")
Callback functions
---------------------------------------
"""
def _connected(self, link_uri):
"""
This callback is executed as soon as the connection to the
......@@ -349,31 +299,70 @@ class PPSRadioClient:
# change state to motors OFF
cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
rospy.loginfo("Connection to %s successful: " % link_uri)
rospy.loginfo("[CrazyRadio] Connection to %s successful: " % link_uri)
# Config for Logging
self._start_logging()
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.change_status_to(DISCONNECTED)
rospy.logwarn("[CrazyRadio] Disconnected from %s" % link_uri)
# change state to motors OFF
self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF)
self._stop_logging()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the specified address)"""
self.change_status_to(DISCONNECTED)
rospy.logerr("Connection to %s failed: %s" % (link_uri, msg))
rospy.logerr("[CrazyRadio] Connection to %s failed: %s" % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
self.change_status_to(DISCONNECTED)
rospy.logerr("Connection to %s lost: %s" % (link_uri, msg))
rospy.logerr("[CrazyRadio] Connection to %s lost: %s" % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
self.change_status_to(DISCONNECTED)
rospy.logwarn("Disconnected from %s" % link_uri)
"""
---------------------------------------
CrazyRadio interface functions
---------------------------------------
"""
def change_status_to(self, new_status):
print "status changed to: %s" % new_status
self._status = new_status
self.status_pub.publish(new_status)
def get_status(self):
return self._status
def update_link_uri(self):
self.link_uri = "radio://" + rospy.get_param("~crazyFlieAddress")
def connect(self):
# update link from ros params
self.update_link_uri()
print "Connecting to %s" % self.link_uri
self.change_status_to(CONNECTING)
rospy.loginfo("connecting...")
self._cf.open_link(self.link_uri)