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 aa8a813040a3ca82f157580f6a7796fb46d79f9c..534f96edc409d4f168a2bd94f1f20587cb169387 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 @@ -33,4 +33,5 @@ void MainWindow::on_RF_Connect_button_clicked() std_msgs::Int32 msg; msg.data = CMD_RECONNECT; crazyRadioCommandPublisher.publish(msg); + ROS_INFO("command reconnect published"); } diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread.cpp index 72a2e5b52792d804e02200c90eb11d29ee458a07..e8a349ae14eda411956d63828e3cdc5f4b6d9732 100644 --- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread.cpp +++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/rosNodeThread.cpp @@ -72,7 +72,7 @@ void rosNodeThread::run() // cmd_msg.linear.x = m_speed; // cmd_msg.angular.z = m_angle; pMutex->unlock(); - ROS_INFO("RUNNING"); + // ROS_INFO("RUNNING"); // sim_velocity.publish(cmd_msg); ros::spinOnce(); diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py index ad0c60f76c182d8d5656127a8231f222a18d84df..0c712613f376ea47d3da1190f11d606cae8740b2 100755 --- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py +++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py @@ -83,7 +83,7 @@ class PPSRadioClient: # Connect to the Crazyflie print "Connecting to %s" % link_uri - self.connect(); + # self.connect(); def connect(self): self.status = CONNECTING @@ -163,11 +163,12 @@ 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(data): """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 self.status == DISCONNECTED: - self.connect() + if cf_client.status == DISCONNECTED: + cf_client.connect() def controlCommandCallback(data): """Callback for controller actions""" @@ -194,15 +195,16 @@ if __name__ == '__main__': rospy.loginfo("Crazyradio connecting to %s" % radio_address) #use this following two lines to connect without data from CentralManager - #radio_address = "radio://0/72/2M" - #rospy.loginfo("manual address loaded") + # radio_address = "radio://0/72/2M" + # rospy.loginfo("manual address loaded") + global cfbattery_pub cfbattery_pub = rospy.Publisher('cfbattery', Float32, queue_size=10) global cf_client cf_client = PPSRadioClient(radio_address) - rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, cf_client.crazyRadioCommandCallback) + rospy.Subscriber("PPSClient/crazyRadioCommand", Int32, crazyRadioCommandCallback) time.sleep(1.0)