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 c1bd7a8eaf2be3a1edee589fa827912b9b0e0c35..66a4291ec069955a364ac43c1cee1e38603f9043 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 @@ -8,13 +8,15 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) : QMainWindow(parent), ui(new Ui::MainWindow), - m_radio_status(DISCONNECTED), m_battery_level(0) { m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI"); ui->setupUi(this); m_rosNodeThread->init(); + setCrazyRadioStatus(DISCONNECTED); + + std::string ros_namespace = ros::this_node::getNamespace(); ROS_INFO("namespace: %s", ros_namespace.c_str()); @@ -45,13 +47,10 @@ MainWindow::~MainWindow() void MainWindow::disableGUI() { - ui->battery_bar->setValue(0); - ui->battery_bar->setEnabled(false); } void MainWindow::enableGUI() { - ui->battery_bar->setEnabled(true); } void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg) @@ -118,6 +117,13 @@ float MainWindow::fromVoltageToPercent(float voltage) } float percentage = 100.0 * m_battery_level/num_cutoffs; + + // should not hapen, but just in case... + if(percentage > 100) + percentage = 100; + if(percentage < 0) + percentage = 0; + return percentage; } diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py index 886c13cbbf10e37b5a9d9d73f5af17b0656ca574..4b4f945b43ddfa790dc2023ef31549fd0dd96026 100755 --- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py +++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py @@ -47,6 +47,13 @@ DISCONNECTED = 2 # Commands coming CMD_RECONNECT = 0 +# Commands for PPSClient +CMD_USE_SAFE_CONTROLLER = 1 +CMD_USE_CUSTOM_CONTROLLER = 2 +CMD_CRAZYFLY_TAKE_OFF = 3 +CMD_CRAZYFLY_LAND = 4 +CMD_CRAZYFLY_MOTORS_OFF = 5 + rp = RosPack() record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag' rospy.loginfo('afdsasdfasdfsadfasdfasdfasdfasdfasdfasdf') @@ -73,6 +80,7 @@ class PPSRadioClient: self.link_uri = link_uri self.status_pub = rospy.Publisher(node_name + '/CrazyRadioStatus', Int32, queue_size=1) + self.PPSClient_command_pub = rospy.Publisher('PPSClient/Command', Int32, queue_size=1) # Initialize the CrazyFlie and add callbacks self._init_cf() @@ -155,11 +163,16 @@ class PPSRadioClient: quadrotor is established. """ self.change_status_to(CONNECTED) + # change state to motors OFF + cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF) + rospy.loginfo("Connection to %s successful: " % link_uri) # 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)""" @@ -177,6 +190,9 @@ class PPSRadioClient: self.change_status_to(DISCONNECTED) rospy.logwarn("Disconnected from %s" % link_uri) + # change state to motors OFF + self.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF) + self.logconf.stop() rospy.loginfo("logconf stopped") self.logconf.delete() @@ -247,9 +263,13 @@ if __name__ == '__main__': rospy.loginfo("Turning off crazyflie") cf_client._send_to_commander(0, 0, 0, 0, 0, 0, 0, 0, CONTROLLER_MOTOR) + # change state to motors OFF + cf_client.PPSClient_command_pub.publish(CMD_CRAZYFLY_MOTORS_OFF) #wait for client to send its commands time.sleep(1.0) + + bag.close() rospy.loginfo("bag closed") diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp index c2415fceca31f1c934fdd5b6425b75a8af47afb5..6cbad5bd4b90c3e9a7de9f82f35d576ad39db93f 100755 --- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp +++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp @@ -243,6 +243,7 @@ void viconCallback(const ViconData& viconData) { if(changed_state_flag) // stuff that will be run only once when changing state { changed_state_flag = false; + ROS_INFO("STATE_MOTORS_OFF"); } break; case STATE_TAKE_OFF: //we need to move up from where we are now. @@ -252,6 +253,7 @@ void viconCallback(const ViconData& viconData) { takeOffCF(local); counter = 0; finished_take_off = false; + ROS_INFO("STATE_TAKE_OFF"); } counter++; if(counter >= DURATION_TAKE_OFF) @@ -271,6 +273,7 @@ void viconCallback(const ViconData& viconData) { changed_state_flag = false; // need to change setpoint to the one from file goToOrigin(); + ROS_INFO("STATE_FLYING"); } break; case STATE_LAND: @@ -280,6 +283,7 @@ void viconCallback(const ViconData& viconData) { landCF(local); counter = 0; finished_land = false; + ROS_INFO("STATE_LAND"); } counter++; if(counter >= DURATION_LANDING) @@ -297,7 +301,6 @@ void viconCallback(const ViconData& viconData) { break; } - // control part that should always be running, calls to controller, computation of control output if(flying_state != STATE_MOTORS_OFF) {