diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h index 4115f39772cb355b9ca1f68a2af84eb860d1f923..892373945d90e04dfd7c981af6081f9864aa32d9 100644 --- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h +++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h @@ -29,6 +29,8 @@ private: Ui::MainWindow *ui; rosNodeThread* m_rosNodeThread; + + ros::Publisher crazyRadioCommandPublisher; }; #endif // MAINWINDOW_H diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp index 534f96edc409d4f168a2bd94f1f20587cb169387..f045fb5211933a9451e59650d86a8f33e15b11ca 100644 --- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp +++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp @@ -14,6 +14,10 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : m_rosNodeThread->init(); qRegisterMetaType<ptrToMessage>("ptrToMessage"); QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&))); + + ros::NodeHandle nh("~"); + + crazyRadioCommandPublisher = nh.advertise<std_msgs::Int32>("crazyRadioCommand", 1); } MainWindow::~MainWindow() @@ -27,11 +31,8 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne void MainWindow::on_RF_Connect_button_clicked() { - ros::NodeHandle nh("~"); - ros::Publisher crazyRadioCommandPublisher = nh.advertise<std_msgs::Int32>("crazyRadioCommand", 1); - std_msgs::Int32 msg; msg.data = CMD_RECONNECT; - crazyRadioCommandPublisher.publish(msg); + this->crazyRadioCommandPublisher.publish(msg); ROS_INFO("command reconnect published"); } diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py index 0c712613f376ea47d3da1190f11d606cae8740b2..75b5191ef8cfacc0dbe5e4a21ef18730e9167b53 100755 --- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py +++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py @@ -70,6 +70,7 @@ class PPSRadioClient: self.motor3cmd = 0.0 self.motor4cmd = 0.0 self.status = DISCONNECTED + self.link_uri = link_uri # Initialize the CrazyFlie and add callbacks self._cf = Crazyflie() @@ -87,7 +88,8 @@ class PPSRadioClient: def connect(self): self.status = CONNECTING - self._cf.open_link(link_uri) + rospy.loginfo("connecting...") + self._cf.open_link(self.link_uri) def _data_received_callback(self, timestamp, data, logconf): #print "log of stabilizer and battery: [%d][%s]: %s" % (timestamp, logconf.name, data) @@ -141,11 +143,13 @@ class PPSRadioClient: def _connection_failed(self, link_uri, msg): """Callback when connection initial connection fails (i.e no Crazyflie at the specified address)""" + self.status = DISCONNECTED rospy.logerr("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.status = DISCONNECTED rospy.logerr("Connection to %s lost: %s" % (link_uri, msg)) def _disconnected(self, link_uri): @@ -163,12 +167,15 @@ class PPSRadioClient: pk.data = struct.pack('<fffHHHHHH', roll * RAD_TO_DEG, pitch * RAD_TO_DEG, yaw * RAD_TO_DEG, thrust, cmd1, cmd2, cmd3, cmd4, mode) self._cf.send_packet(pk) -def crazyRadioCommandCallback(data): + def crazyRadioCommandCallback(self, msg): """Callback to tell CrazyRadio to reconnect""" - print "crazyRadio command received" - if data == CMD_RECONNECT: # reconnect, check status first and then do whatever needs to be done - if cf_client.status == DISCONNECTED: - cf_client.connect() + print "crazyRadio command received %s" % msg.data + if msg.data == CMD_RECONNECT: # reconnect, check status first and then do whatever needs to be done + print "entered reconnect" + print "status: %s" % self.status + if self.status == DISCONNECTED: + print "entered disconnected" + self.connect() def controlCommandCallback(data): """Callback for controller actions""" @@ -204,7 +211,8 @@ if __name__ == '__main__': global cf_client cf_client = PPSRadioClient(radio_address) - rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, crazyRadioCommandCallback) + rospy.Subscriber("student_GUI/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from student_GUI + rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) # allows commands from scripts time.sleep(1.0)