From 0e402598065c54bdfd01d0adb4cda309dda9c75a Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Mar 2020 15:26:31 -0600 Subject: [PATCH 1/6] Allow option to wait for serial connection --- rosflight/CMakeLists.txt | 2 +- rosflight/include/rosflight/rosflight_io.h | 5 +++ rosflight/src/mavrosflight/mavlink_comm.cpp | 4 --- rosflight/src/rosflight_io.cpp | 35 ++++++++++++++++----- 4 files changed, 33 insertions(+), 13 deletions(-) diff --git a/rosflight/CMakeLists.txt b/rosflight/CMakeLists.txt index ca64a52f..491d7b5c 100644 --- a/rosflight/CMakeLists.txt +++ b/rosflight/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(rosflight) -set(CMAKE_BUILD_TYPE Release) +set(CMAKE_BUILD_TYPE Debug) message("CMAKE_C_FLAGS_RELEASE is ${CMAKE_C_FLAGS_RELEASE}") diff --git a/rosflight/include/rosflight/rosflight_io.h b/rosflight/include/rosflight/rosflight_io.h index efa226f6..ce6bdca8 100644 --- a/rosflight/include/rosflight/rosflight_io.h +++ b/rosflight/include/rosflight/rosflight_io.h @@ -162,6 +162,11 @@ class rosflightIO : return value < min ? min : (value > max ? max : value); } + // parameters + bool wait_for_serial_{false}; + float serial_check_period_s_{1.0}; + + ros::NodeHandle nh_; diff --git a/rosflight/src/mavrosflight/mavlink_comm.cpp b/rosflight/src/mavrosflight/mavlink_comm.cpp index 3f3f334c..e9cbe157 100644 --- a/rosflight/src/mavrosflight/mavlink_comm.cpp +++ b/rosflight/src/mavrosflight/mavlink_comm.cpp @@ -68,10 +68,6 @@ void MavlinkComm::close() io_service_.stop(); do_close(); - if (io_thread_.joinable()) - { - io_thread_.join(); - } } void MavlinkComm::register_mavlink_listener(MavlinkListenerInterface * const listener) diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp index f6c296ce..4cf120c6 100644 --- a/rosflight/src/rosflight_io.cpp +++ b/rosflight/src/rosflight_io.cpp @@ -65,6 +65,7 @@ rosflightIO::rosflightIO() calibrate_rc_srv_ = nh_.advertiseService("calibrate_rc_trim", &rosflightIO::calibrateRCTrimSrvCallback, this); reboot_srv_ = nh_.advertiseService("reboot", &rosflightIO::rebootSrvCallback, this); reboot_bootloader_srv_ = nh_.advertiseService("reboot_to_bootloader", &rosflightIO::rebootToBootloaderSrvCallback, this); + ros::NodeHandle nh_private("~"); @@ -84,20 +85,38 @@ rosflightIO::rosflightIO() std::string port = nh_private.param("port", "/dev/ttyACM0"); int baud_rate = nh_private.param("baud_rate", 921600); + wait_for_serial_ = nh_private.param("wait_for_serial", false); + serial_check_period_s_ = nh_private.param("serial_check_period", 1.0); ROS_INFO("Connecting to serial port \"%s\", at %d baud", port.c_str(), baud_rate); mavlink_comm_ = new mavrosflight::MavlinkSerial(port, baud_rate); } - try - { - mavlink_comm_->open(); //! \todo move this into the MavROSflight constructor - mavrosflight_ = new mavrosflight::MavROSflight(*mavlink_comm_); - } - catch (mavrosflight::SerialException e) + bool connected = false; + while(!connected) { - ROS_FATAL("%s", e.what()); - ros::shutdown(); + try + { + mavlink_comm_->open(); //! \todo move this into the MavROSflight constructor + mavrosflight_ = new mavrosflight::MavROSflight(*mavlink_comm_); + connected = true; + } + catch (mavrosflight::SerialException e) + { + connected = false; + if(wait_for_serial_) + { + ROS_WARN("%s", e.what()); + ROS_WARN("Retrying connection in %f s", serial_check_period_s_); + ros::Duration(serial_check_period_s_).sleep(); + } + else + { + ROS_FATAL("%s", e.what()); + ros::shutdown(); + return; + } + } } mavrosflight_->comm.register_mavlink_listener(this); From 9f565482202b5ece63e2b5ac6df8a7300f96c7ef Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Mar 2020 15:40:10 -0600 Subject: [PATCH 2/6] Show error message on disconnect instead of crashing --- rosflight/include/rosflight/mavrosflight/mavlink_comm.h | 5 +++++ .../rosflight/mavrosflight/mavlink_listener_interface.h | 1 + rosflight/include/rosflight/rosflight_io.h | 1 + rosflight/src/mavrosflight/mavlink_comm.cpp | 6 ++++++ rosflight/src/rosflight_io.cpp | 6 ++++++ 5 files changed, 19 insertions(+) diff --git a/rosflight/include/rosflight/mavrosflight/mavlink_comm.h b/rosflight/include/rosflight/mavrosflight/mavlink_comm.h index cc219c02..df80b39d 100644 --- a/rosflight/include/rosflight/mavrosflight/mavlink_comm.h +++ b/rosflight/include/rosflight/mavrosflight/mavlink_comm.h @@ -171,6 +171,11 @@ class MavlinkComm */ void async_write_end(const boost::system::error_code& error, size_t bytes_transferred); + /** + * \brief Notifies all listeners that the connection has been broken + */ + void notify_disconnect(); + //=========================================================================== // member variables //=========================================================================== diff --git a/rosflight/include/rosflight/mavrosflight/mavlink_listener_interface.h b/rosflight/include/rosflight/mavrosflight/mavlink_listener_interface.h index 91238e78..d264fc46 100644 --- a/rosflight/include/rosflight/mavrosflight/mavlink_listener_interface.h +++ b/rosflight/include/rosflight/mavrosflight/mavlink_listener_interface.h @@ -54,6 +54,7 @@ class MavlinkListenerInterface * \param msg The mavlink message to handle */ virtual void handle_mavlink_message(const mavlink_message_t &msg) = 0; + virtual void on_mavlink_disconnect(){}; }; } // namespace mavrosflight diff --git a/rosflight/include/rosflight/rosflight_io.h b/rosflight/include/rosflight/rosflight_io.h index ce6bdca8..61a45d77 100644 --- a/rosflight/include/rosflight/rosflight_io.h +++ b/rosflight/include/rosflight/rosflight_io.h @@ -96,6 +96,7 @@ class rosflightIO : ~rosflightIO(); virtual void handle_mavlink_message(const mavlink_message_t &msg); + virtual void on_mavlink_disconnect() override; virtual void on_new_param_received(std::string name, double value); virtual void on_param_value_updated(std::string name, double value); diff --git a/rosflight/src/mavrosflight/mavlink_comm.cpp b/rosflight/src/mavrosflight/mavlink_comm.cpp index e9cbe157..6599de0a 100644 --- a/rosflight/src/mavrosflight/mavlink_comm.cpp +++ b/rosflight/src/mavrosflight/mavlink_comm.cpp @@ -68,6 +68,7 @@ void MavlinkComm::close() io_service_.stop(); do_close(); + notify_disconnect(); } void MavlinkComm::register_mavlink_listener(MavlinkListenerInterface * const listener) @@ -140,6 +141,11 @@ void MavlinkComm::async_read_end(const boost::system::error_code &error, size_t async_read(); } +void MavlinkComm::notify_disconnect() +{ + for(MavlinkListenerInterface *listener: listeners_) + listener->on_mavlink_disconnect(); +} void MavlinkComm::send_message(const mavlink_message_t &msg) { diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp index 4cf120c6..d4e416a0 100644 --- a/rosflight/src/rosflight_io.cpp +++ b/rosflight/src/rosflight_io.cpp @@ -228,6 +228,12 @@ void rosflightIO::handle_mavlink_message(const mavlink_message_t &msg) } } +void rosflightIO::on_mavlink_disconnect() +{ + ROS_FATAL("Connection to firmware lost. Shutting down."); + ros::shutdown(); +} + void rosflightIO::on_new_param_received(std::string name, double value) { ROS_DEBUG("Got parameter %s with value %g", name.c_str(), value); From 7ee04f40b829422ae53df348a7f13e2cf77a595e Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Mar 2020 15:58:49 -0600 Subject: [PATCH 3/6] Restructure code to support better disconnect handling --- rosflight/include/rosflight/rosflight_io.h | 5 ++ rosflight/src/rosflight_io.cpp | 61 ++++++++++++++++------ 2 files changed, 49 insertions(+), 17 deletions(-) diff --git a/rosflight/include/rosflight/rosflight_io.h b/rosflight/include/rosflight/rosflight_io.h index 61a45d77..155d3573 100644 --- a/rosflight/include/rosflight/rosflight_io.h +++ b/rosflight/include/rosflight/rosflight_io.h @@ -107,6 +107,11 @@ class rosflightIO : static constexpr float PARAMETER_PERIOD = 3; //Time between parameter requests private: + // Initialization and Re-initialization + bool attempt_connect(); + void finish_setup(); + void handle_disconnect(); + void cleanup_connection(); // handle mavlink messages void handle_heartbeat_msg(const mavlink_message_t &msg); diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp index d4e416a0..ff7b67e1 100644 --- a/rosflight/src/rosflight_io.cpp +++ b/rosflight/src/rosflight_io.cpp @@ -92,6 +92,30 @@ rosflightIO::rosflightIO() mavlink_comm_ = new mavrosflight::MavlinkSerial(port, baud_rate); } + if(!attempt_connect()) + { + ros::shutdown(); + return; + } + + mavrosflight_->comm.register_mavlink_listener(this); + mavrosflight_->param.register_param_listener(this); + + // Set up a few other random things + frame_id_ = nh_private.param("frame_id", "world"); + + prev_status_.armed = false; + prev_status_.failsafe = false; + prev_status_.rc_override = false; + prev_status_.offboard = false; + prev_status_.control_mode = OFFBOARD_CONTROL_MODE_ENUM_END; + prev_status_.error_code = ROSFLIGHT_ERROR_NONE; + + finish_setup(); +} + +bool rosflightIO::attempt_connect() +{ bool connected = false; while(!connected) { @@ -113,15 +137,15 @@ rosflightIO::rosflightIO() else { ROS_FATAL("%s", e.what()); - ros::shutdown(); - return; + return false; } } } + return true; +} - mavrosflight_->comm.register_mavlink_listener(this); - mavrosflight_->param.register_param_listener(this); - +void rosflightIO::finish_setup() +{ // request the param list mavrosflight_->param.request_params(); param_timer_ = nh_.createTimer(ros::Duration(PARAMETER_PERIOD), &rosflightIO::paramTimerCallback, this); @@ -135,18 +159,22 @@ rosflightIO::rosflightIO() unsaved_msg.data = false; unsaved_params_pub_.publish(unsaved_msg); - // Set up a few other random things - frame_id_ = nh_private.param("frame_id", "world"); - - prev_status_.armed = false; - prev_status_.failsafe = false; - prev_status_.rc_override = false; - prev_status_.offboard = false; - prev_status_.control_mode = OFFBOARD_CONTROL_MODE_ENUM_END; - prev_status_.error_code = ROSFLIGHT_ERROR_NONE; - //Start the heartbeat heartbeat_timer_ = nh_.createTimer(ros::Duration(HEARTBEAT_PERIOD), &rosflightIO::heartbeatTimerCallback, this); + +} + +void rosflightIO::handle_disconnect() +{ + ROS_FATAL("Connection to firmware lost. Shutting down."); + ros::shutdown(); +} + +void rosflightIO::cleanup_connection() +{ + param_timer_.stop(); + version_timer_.stop(); + heartbeat_timer_.stop(); } rosflightIO::~rosflightIO() @@ -230,8 +258,7 @@ void rosflightIO::handle_mavlink_message(const mavlink_message_t &msg) void rosflightIO::on_mavlink_disconnect() { - ROS_FATAL("Connection to firmware lost. Shutting down."); - ros::shutdown(); + handle_disconnect(); } void rosflightIO::on_new_param_received(std::string name, double value) From af61c68e63cd2102c575951a6a868de8b90eea34 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Mar 2020 16:06:03 -0600 Subject: [PATCH 4/6] Handle ctrl-c while looking for connection better --- rosflight/src/rosflight_io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp index ff7b67e1..2a83b182 100644 --- a/rosflight/src/rosflight_io.cpp +++ b/rosflight/src/rosflight_io.cpp @@ -117,7 +117,7 @@ rosflightIO::rosflightIO() bool rosflightIO::attempt_connect() { bool connected = false; - while(!connected) + while(!connected && ros::ok()) { try { @@ -141,7 +141,7 @@ bool rosflightIO::attempt_connect() } } } - return true; + return ros::ok(); } void rosflightIO::finish_setup() From e8b1541df207f6772e03a85f7632adafa5049235 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Thu, 19 Mar 2020 16:24:02 -0600 Subject: [PATCH 5/6] Option to reconnect on lost connection. Not working fully --- rosflight/include/rosflight/rosflight_io.h | 2 +- rosflight/src/rosflight_io.cpp | 19 ++++++++++++++++--- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/rosflight/include/rosflight/rosflight_io.h b/rosflight/include/rosflight/rosflight_io.h index 155d3573..88c6c39c 100644 --- a/rosflight/include/rosflight/rosflight_io.h +++ b/rosflight/include/rosflight/rosflight_io.h @@ -171,7 +171,7 @@ class rosflightIO : // parameters bool wait_for_serial_{false}; float serial_check_period_s_{1.0}; - + bool do_reconnect_{false}; ros::NodeHandle nh_; diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp index 2a83b182..ae379c18 100644 --- a/rosflight/src/rosflight_io.cpp +++ b/rosflight/src/rosflight_io.cpp @@ -66,8 +66,8 @@ rosflightIO::rosflightIO() reboot_srv_ = nh_.advertiseService("reboot", &rosflightIO::rebootSrvCallback, this); reboot_bootloader_srv_ = nh_.advertiseService("reboot_to_bootloader", &rosflightIO::rebootToBootloaderSrvCallback, this); - ros::NodeHandle nh_private("~"); + do_reconnect_ = nh_private.param("do_reconnect", false); if (nh_private.param("udp", false)) { @@ -166,8 +166,21 @@ void rosflightIO::finish_setup() void rosflightIO::handle_disconnect() { - ROS_FATAL("Connection to firmware lost. Shutting down."); - ros::shutdown(); + if(do_reconnect_) + { + ROS_ERROR("Connection to firmware lost. Attempting to reconnect"); + cleanup_connection(); + wait_for_serial_ = true; + attempt_connect(); + wait_for_serial_ = false; + ROS_INFO("Connection regained"); + finish_setup(); + } + else + { + ROS_FATAL("Connection to firmware lost. Shutting down."); + ros::shutdown(); + } } void rosflightIO::cleanup_connection() From 63d3e376f7665f948c0fa9d0d0be1ff6be812b61 Mon Sep 17 00:00:00 2001 From: Trey Henrichsen Date: Fri, 20 Mar 2020 14:13:30 -0600 Subject: [PATCH 6/6] Clean up reconnection after lost connection --- rosflight/include/rosflight/rosflight_io.h | 4 ++-- rosflight/src/rosflight_io.cpp | 14 +++++++------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rosflight/include/rosflight/rosflight_io.h b/rosflight/include/rosflight/rosflight_io.h index 88c6c39c..66e27656 100644 --- a/rosflight/include/rosflight/rosflight_io.h +++ b/rosflight/include/rosflight/rosflight_io.h @@ -108,7 +108,7 @@ class rosflightIO : private: // Initialization and Re-initialization - bool attempt_connect(); + bool attempt_connect(bool do_retry, float retry_delay); void finish_setup(); void handle_disconnect(); void cleanup_connection(); @@ -170,7 +170,7 @@ class rosflightIO : // parameters bool wait_for_serial_{false}; - float serial_check_period_s_{1.0}; + float serial_retry_delay_s_{1.0}; bool do_reconnect_{false}; diff --git a/rosflight/src/rosflight_io.cpp b/rosflight/src/rosflight_io.cpp index ae379c18..944ff960 100644 --- a/rosflight/src/rosflight_io.cpp +++ b/rosflight/src/rosflight_io.cpp @@ -86,13 +86,13 @@ rosflightIO::rosflightIO() int baud_rate = nh_private.param("baud_rate", 921600); wait_for_serial_ = nh_private.param("wait_for_serial", false); - serial_check_period_s_ = nh_private.param("serial_check_period", 1.0); + serial_retry_delay_s_ = nh_private.param("serial_check_period", 1.0); ROS_INFO("Connecting to serial port \"%s\", at %d baud", port.c_str(), baud_rate); mavlink_comm_ = new mavrosflight::MavlinkSerial(port, baud_rate); } - if(!attempt_connect()) + if(!attempt_connect(wait_for_serial_, serial_retry_delay_s_)) { ros::shutdown(); return; @@ -114,7 +114,7 @@ rosflightIO::rosflightIO() finish_setup(); } -bool rosflightIO::attempt_connect() +bool rosflightIO::attempt_connect(bool do_retry, float retry_delay) { bool connected = false; while(!connected && ros::ok()) @@ -128,11 +128,11 @@ bool rosflightIO::attempt_connect() catch (mavrosflight::SerialException e) { connected = false; - if(wait_for_serial_) + if(do_retry) { ROS_WARN("%s", e.what()); - ROS_WARN("Retrying connection in %f s", serial_check_period_s_); - ros::Duration(serial_check_period_s_).sleep(); + ROS_WARN("Retrying connection in %f s", retry_delay); + ros::Duration(retry_delay).sleep(); } else { @@ -171,7 +171,7 @@ void rosflightIO::handle_disconnect() ROS_ERROR("Connection to firmware lost. Attempting to reconnect"); cleanup_connection(); wait_for_serial_ = true; - attempt_connect(); + attempt_connect(do_reconnect_, serial_retry_delay_s_); wait_for_serial_ = false; ROS_INFO("Connection regained"); finish_setup();