diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..50a6a3e --- /dev/null +++ b/.clang-format @@ -0,0 +1,14 @@ +BasedOnStyle: LLVM +AllowShortBlocksOnASingleLine: Always +AllowShortLoopsOnASingleLine: true +AlwaysBreakTemplateDeclarations: Yes +BreakBeforeBraces: Linux +ColumnLimit: 100 +IndentPPDirectives: BeforeHash +PenaltyBreakComment: 1000 +PenaltyExcessCharacter: 10 +SpaceAfterTemplateKeyword: false +SpaceInEmptyBlock: true +SpacesInConditionalStatement: true +SpacesInParentheses: true +UseTab: Never diff --git a/.github/workflows/lint_build_test.yaml b/.github/workflows/lint_build_test.yaml new file mode 100644 index 0000000..a187edb --- /dev/null +++ b/.github/workflows/lint_build_test.yaml @@ -0,0 +1,66 @@ +name: CI Pipeline + +on: + push: + branches: + - ros2 + pull_request: + branches: + - ros2 +defaults: + run: + shell: bash + +jobs: + Linting: + name: Lint Code Base + runs-on: ubuntu-latest + steps: + - name: Checkout Code + uses: actions/checkout@v4 + with: + fetch-depth: 0 + - name: Setup + run: | + sudo apt-get update && sudo apt-get install -y clang-format cppcheck libxml2-utils + - name: Lint C++ code + run: | + echo "Linting C++ code..." + clang-format -Werror -n -style=file $(find . -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -o -name '*.c' -o -name '*.cc') + + - name: Run cppcheck on C++ code + run: | + echo "Running cppcheck on C++ code..." + cppcheck --force --quiet --error-exitcode=1 --language=c++ $(find . -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -o -name '*.c' -o -name '*.cc') + + - name: Lint package.xml + run: | + xmllint --noout --schema http://download.ros.org/schema/package_format3.xsd $(find . -name 'package.xml') + + + build-and-test: + needs: Linting + runs-on: ubuntu-latest + container: + image: ros:jazzy-ros-base + steps: + - name: install build tools + run: | + sudo apt-get update + sudo apt-get install -y ros-dev-tools + - uses: actions/checkout@v4 + with: + path: src/e_stop_manager + - name: rosdep + run: | + rosdep update --rosdistro jazzy --include-eol-distros + rosdep install -y --from-paths src --ignore-src --rosdistro jazzy + - name: build + run: | + source /opt/ros/jazzy/setup.bash + colcon build + - name: test + run: | + source /opt/ros/jazzy/setup.bash + colcon test --packages-select e_stop_manager + colcon test-result --verbose diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..cc9b2da --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,25 @@ +repos: + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v14.0.6 + hooks: + - id: clang-format + name: Clang Format + args: [--style=file] + files: \.(cpp|hpp|h|c|cc)$ + + - repo: local + hooks: + - id: cppcheck + name: Cppcheck + entry: cppcheck --force --quiet --language=c++ + language: system + types: [c, c++] + files: \.(cpp|hpp|h|c|cc)$ + + - repo: https://github.com/psf/black + rev: 23.1.0 + hooks: + - id: black + name: Black + args: [--diff] + language_version: python3 diff --git a/e_stop_manager/include/e_stop_manager/e_stop_manager.h b/e_stop_manager/include/e_stop_manager/e_stop_manager.h index 2851b0e..7da0d65 100644 --- a/e_stop_manager/include/e_stop_manager/e_stop_manager.h +++ b/e_stop_manager/include/e_stop_manager/e_stop_manager.h @@ -1,34 +1,34 @@ -#include "rclcpp/rclcpp.hpp" #include "rclcpp/publisher.hpp" -#include -#include +#include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" +#include +#include namespace e_stop_manager { - class EStopManager - { - public: - explicit EStopManager(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - // required if not subclassing Node, see https://github.com/ros2/demos/blob/humble/composition/src/node_like_listener_component.cpp - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const { - return this->node_->get_node_base_interface(); - } - private: - void publishEStops( bool force_e_stop = false ); +class EStopManager +{ +public: + explicit EStopManager( const rclcpp::NodeOptions &options = rclcpp::NodeOptions() ); - bool setEStopServiceCB(const std::shared_ptr request, - std::shared_ptr response ); + // required if not subclassing Node, see https://github.com/ros2/demos/blob/humble/composition/src/node_like_listener_component.cpp + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const + { + return this->node_->get_node_base_interface(); + } +private: + void publishEStops( bool force_e_stop = false ); + bool setEStopServiceCB( const std::shared_ptr request, + std::shared_ptr response ); - rclcpp::Service::SharedPtr set_e_stop_service_; + rclcpp::Service::SharedPtr set_e_stop_service_; - e_stop_manager_msgs::msg::EStopList e_stop_list_msg_; - rclcpp::Publisher::SharedPtr e_stop_list_pub_; - std::map>, std::vector > e_stop_pub_; - rclcpp::Node::SharedPtr node_; - }; -} \ No newline at end of file + e_stop_manager_msgs::msg::EStopList e_stop_list_msg_; + rclcpp::Publisher::SharedPtr e_stop_list_pub_; + std::map>, std::vector> e_stop_pub_; + rclcpp::Node::SharedPtr node_; +}; +} // namespace e_stop_manager \ No newline at end of file diff --git a/e_stop_manager/package.xml b/e_stop_manager/package.xml index 5a25502..cee1206 100644 --- a/e_stop_manager/package.xml +++ b/e_stop_manager/package.xml @@ -1,26 +1,20 @@ - + e_stop_manager 1.0.0 The e_stop_manager package - Katrin + Aljoscha Schmidt - - - TODO - ament_cmake std_msgs e_stop_manager_msgs rclcpp_components ros_testing - - ament_cmake diff --git a/e_stop_manager/src/e_stop_manager.cpp b/e_stop_manager/src/e_stop_manager.cpp index 5e84d5a..3b7888b 100644 --- a/e_stop_manager/src/e_stop_manager.cpp +++ b/e_stop_manager/src/e_stop_manager.cpp @@ -1,181 +1,174 @@ #include "e_stop_manager/e_stop_manager.h" - namespace e_stop_manager { - EStopManager::EStopManager(const rclcpp::NodeOptions & options): - node_(std::make_shared("e_stop_manager", options)) - { - node_->declare_parameter>("e_stop_names",std::vector{}); - node_->declare_parameter>("e_stop_topics",std::vector{}); - e_stop_list_pub_ = node_->create_publisher("e_stop_list", 5); - - rclcpp::Parameter e_stop_topics_param; - std::vector e_stop_topics; - std::vector e_stop_names; - // load e-stop topics and names - if (node_->get_parameter("e_stop_topics", e_stop_topics_param) && e_stop_topics_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) - { - e_stop_topics = e_stop_topics_param.as_string_array(); - } - else - { - RCLCPP_ERROR(rclcpp::get_logger("e_stop_manager"), "Parameter e_stop_topic not provided or is not a list! E-stops will only be published on e-stop-list topic!"); - } - - if (node_->get_parameter("e_stop_names", e_stop_topics_param) && e_stop_topics_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) - { - e_stop_names = e_stop_topics_param.as_string_array(); - } - else - { - RCLCPP_ERROR(rclcpp::get_logger("e_stop_manager"), "Parameter e_stop_topic not provided or is not a list! E-stops will only be published on e-stop-list topic!"); - } - - // declare parameters per e-stop - for(const auto& e_stop_name: e_stop_names) - { - node_->declare_parameter(e_stop_name+"_start_value", false); - node_->declare_parameter(e_stop_name+"_topic", e_stop_topics.empty()?"missing_topics":e_stop_topics[0]); - } - std::map> e_stops_per_topic; - std::map e_stops_start_values; - for(const auto& e_stop_name:e_stop_names) - { - e_stops_start_values[e_stop_name] = node_->get_parameter(e_stop_name+"_start_value").as_bool(); - std::string topic = node_->get_parameter(e_stop_name+"_topic").as_string(); - if(e_stops_per_topic.find(topic) != e_stops_per_topic.end()) - { - e_stops_per_topic[topic].push_back(e_stop_name); - } - else - { - e_stops_per_topic[topic] = {e_stop_name}; - } - } - - // create publishers - for (auto& entry : e_stop_topics) - { - std::shared_ptr> pub = node_->create_publisher(entry, 5); - - e_stop_pub_.emplace(pub,e_stops_per_topic[entry]); - } - - // create messages - for (auto& e_stop_name : e_stop_names) - { - e_stop_list_msg_.names.push_back(e_stop_name); - e_stop_list_msg_.values.push_back(e_stops_start_values[e_stop_name]); - } - - if (e_stop_list_msg_.names.empty()) - { - RCLCPP_ERROR(rclcpp::get_logger("e_stop_manager"), "No valid e_stop in list!"); - publishEStops(true); - return; - } - - for (auto& pub : e_stop_pub_) - { - pub.second.erase(std::remove_if(pub.second.begin(), pub.second.end(), - [&](const std::string& name) - { - auto it = std::find(e_stop_list_msg_.names.begin(), - e_stop_list_msg_.names.end(), name); - if (it == e_stop_list_msg_.names.end()) - { - RCLCPP_WARN(rclcpp::get_logger("e_stop_manager"), - "The e_stop named \"%s\" was in e_stop_topics but not in e_stop_list. Ignored.", name.c_str()); - return true; - } - return false; - }), - pub.second.end()); - } - - set_e_stop_service_ = node_->create_service("set_e_stop", std::bind(&EStopManager::setEStopServiceCB, this, std::placeholders::_1, std::placeholders::_2)); - - publishEStops(); - RCLCPP_INFO(rclcpp::get_logger("e_stop_manager"), "e_stop_manager initialized!"); +EStopManager::EStopManager( const rclcpp::NodeOptions &options ) + : node_( std::make_shared( "e_stop_manager", options ) ) +{ + node_->declare_parameter>( "e_stop_names", std::vector{} ); + node_->declare_parameter>( "e_stop_topics", std::vector{} ); + e_stop_list_pub_ = node_->create_publisher( "e_stop_list", 5 ); + + rclcpp::Parameter e_stop_topics_param; + std::vector e_stop_topics; + std::vector e_stop_names; + // load e-stop topics and names + if ( node_->get_parameter( "e_stop_topics", e_stop_topics_param ) && + e_stop_topics_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY ) { + e_stop_topics = e_stop_topics_param.as_string_array(); + } else { + RCLCPP_ERROR( rclcpp::get_logger( "e_stop_manager" ), + "Parameter e_stop_topic not provided or is not a list! E-stops will only be " + "published on e-stop-list topic!" ); + } + + if ( node_->get_parameter( "e_stop_names", e_stop_topics_param ) && + e_stop_topics_param.get_type() == rclcpp::ParameterType::PARAMETER_STRING_ARRAY ) { + e_stop_names = e_stop_topics_param.as_string_array(); + } else { + RCLCPP_ERROR( rclcpp::get_logger( "e_stop_manager" ), + "Parameter e_stop_topic not provided or is not a list! E-stops will only be " + "published on e-stop-list topic!" ); + } + + // declare parameters per e-stop + for ( const auto &e_stop_name : e_stop_names ) { + node_->declare_parameter( e_stop_name + "_start_value", false ); + node_->declare_parameter( + e_stop_name + "_topic", e_stop_topics.empty() ? "missing_topics" : e_stop_topics[0] ); + } + std::map> e_stops_per_topic; + std::map e_stops_start_values; + for ( const auto &e_stop_name : e_stop_names ) { + e_stops_start_values[e_stop_name] = + node_->get_parameter( e_stop_name + "_start_value" ).as_bool(); + std::string topic = node_->get_parameter( e_stop_name + "_topic" ).as_string(); + if ( e_stops_per_topic.find( topic ) != e_stops_per_topic.end() ) { + e_stops_per_topic[topic].push_back( e_stop_name ); + } else { + e_stops_per_topic[topic] = { e_stop_name }; } + } + + // create publishers + for ( auto &entry : e_stop_topics ) { + std::shared_ptr> pub = + node_->create_publisher( entry, 5 ); + + e_stop_pub_.emplace( pub, e_stops_per_topic[entry] ); + } + + // create messages + for ( auto &e_stop_name : e_stop_names ) { + e_stop_list_msg_.names.push_back( e_stop_name ); + e_stop_list_msg_.values.push_back( e_stops_start_values[e_stop_name] ); + } + + if ( e_stop_list_msg_.names.empty() ) { + RCLCPP_ERROR( rclcpp::get_logger( "e_stop_manager" ), "No valid e_stop in list!" ); + publishEStops( true ); + return; + } + + for ( auto &pub : e_stop_pub_ ) { + pub.second.erase( + std::remove_if( + pub.second.begin(), pub.second.end(), + [&]( const std::string &name ) { + auto it = + std::find( e_stop_list_msg_.names.begin(), e_stop_list_msg_.names.end(), name ); + if ( it == e_stop_list_msg_.names.end() ) { + RCLCPP_WARN( + rclcpp::get_logger( "e_stop_manager" ), + "The e_stop named \"%s\" was in e_stop_topics but not in e_stop_list. Ignored.", + name.c_str() ); + return true; + } + return false; + } ), + pub.second.end() ); + } + + set_e_stop_service_ = node_->create_service( + "set_e_stop", std::bind( &EStopManager::setEStopServiceCB, this, std::placeholders::_1, + std::placeholders::_2 ) ); + + publishEStops(); + RCLCPP_INFO( rclcpp::get_logger( "e_stop_manager" ), "e_stop_manager initialized!" ); +} + +bool EStopManager::setEStopServiceCB( + const std::shared_ptr request, + std::shared_ptr response ) +{ - bool EStopManager::setEStopServiceCB( - const std::shared_ptr request, - std::shared_ptr response) - { - - RCLCPP_INFO(rclcpp::get_logger("e_stop_manager"), "EStop '%s' sent value: %d.", request->name.c_str(), request->value); - - bool name_found = false; + RCLCPP_INFO( rclcpp::get_logger( "e_stop_manager" ), "EStop '%s' sent value: %d.", + request->name.c_str(), request->value ); - for (size_t i = 0; i < e_stop_list_msg_.names.size(); ++i) - { - if (e_stop_list_msg_.names[i] == request->name) - { - // if value has not changed, do nothing and return - if (e_stop_list_msg_.values[i] == request->value) - { - RCLCPP_INFO(rclcpp::get_logger("e_stop_manager"), "Do nothing. Requested state already true"); - response->result = response->SUCCESS; - return true; - } + bool name_found = false; - e_stop_list_msg_.values[i] = request->value; - name_found = true; - break; - } - } + for ( size_t i = 0; i < e_stop_list_msg_.names.size(); ++i ) { + if ( e_stop_list_msg_.names[i] == request->name ) { + // if value has not changed, do nothing and return + if ( e_stop_list_msg_.values[i] == request->value ) { + RCLCPP_INFO( rclcpp::get_logger( "e_stop_manager" ), + "Do nothing. Requested state already true" ); + response->result = response->SUCCESS; + return true; + } - if (!name_found) - { + e_stop_list_msg_.values[i] = request->value; + name_found = true; + break; + } + } - RCLCPP_ERROR(rclcpp::get_logger("e_stop_manager"), "EStop : '%s' not found!", request->name.c_str()); - response->result = response->INVALID_ESTOP_NAME; - return true; - } + if ( !name_found ) { - publishEStops(); + RCLCPP_ERROR( rclcpp::get_logger( "e_stop_manager" ), "EStop : '%s' not found!", + request->name.c_str() ); + response->result = response->INVALID_ESTOP_NAME; + return true; + } - RCLCPP_INFO(rclcpp::get_logger("e_stop_manager"), "EStop '%s' sent value: %d.", request->name.c_str(), request->value); + publishEStops(); - response->result = response->SUCCESS; - return true; - } + RCLCPP_INFO( rclcpp::get_logger( "e_stop_manager" ), "EStop '%s' sent value: %d.", + request->name.c_str(), request->value ); - void EStopManager::publishEStops(bool force_e_stop) - { - e_stop_list_pub_->publish(e_stop_list_msg_); - - for (auto& pub : e_stop_pub_) - { - auto msg = std::make_unique(); - msg->data = force_e_stop || - std::any_of( pub.second.begin(), pub.second.end(), - [ & ]( const std::string& name ) - { - // find name (and its index) in list to retrieve its value - auto it = std::find( e_stop_list_msg_.names.begin(), e_stop_list_msg_.names.end(), name ); - if ( it != e_stop_list_msg_.names.end()) - { - int idx = it - e_stop_list_msg_.names.begin(); - return (bool) e_stop_list_msg_.values[idx]; - } - // if name not found, return false - return false; - } ); - - pub.first->publish(std::move(msg)); - } - } + response->result = response->SUCCESS; + return true; +} -}// namespace e_stop_manager +void EStopManager::publishEStops( bool force_e_stop ) +{ + e_stop_list_pub_->publish( e_stop_list_msg_ ); + + for ( auto &pub : e_stop_pub_ ) { + auto msg = std::make_unique(); + msg->data = + force_e_stop || + std::any_of( pub.second.begin(), pub.second.end(), [&]( const std::string &name ) { + // find name (and its index) in list to retrieve its value + auto it = std::find( e_stop_list_msg_.names.begin(), e_stop_list_msg_.names.end(), name ); + if ( it != e_stop_list_msg_.names.end() ) { + int idx = it - e_stop_list_msg_.names.begin(); + return (bool)e_stop_list_msg_.values[idx]; + } + // if name not found, return false + return false; + } ); + + pub.first->publish( std::move( msg ) ); + } +} + +} // namespace e_stop_manager #include "rclcpp_components/register_node_macro.hpp" - // Register the component with class_loader. - // This acts as a sort of entry point, allowing the component to be discoverable when its library - // is being loaded into a running process. - RCLCPP_COMPONENTS_REGISTER_NODE(e_stop_manager::EStopManager); - +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE( e_stop_manager::EStopManager ); diff --git a/e_stop_manager/src/e_stop_manager_node.cpp b/e_stop_manager/src/e_stop_manager_node.cpp index ded97e5..d6cde51 100644 --- a/e_stop_manager/src/e_stop_manager_node.cpp +++ b/e_stop_manager/src/e_stop_manager_node.cpp @@ -1,16 +1,16 @@ -#include #include "e_stop_manager/e_stop_manager.h" +#include -int main(int argc, char** argv) +int main( int argc, char **argv ) { - rclcpp::init(argc, argv); + rclcpp::init( argc, argv ); - rclcpp::executors::SingleThreadedExecutor exe; - auto e_stop_manager = std::make_shared(); - exe.add_node(e_stop_manager->get_node_base_interface()); - exe.spin(); + rclcpp::executors::SingleThreadedExecutor exe; + auto e_stop_manager = std::make_shared(); + exe.add_node( e_stop_manager->get_node_base_interface() ); + exe.spin(); - rclcpp::shutdown(); + rclcpp::shutdown(); - return 0; + return 0; } diff --git a/e_stop_manager/test/test_e_stop_manager.cpp b/e_stop_manager/test/test_e_stop_manager.cpp index f922a38..22d44b6 100644 --- a/e_stop_manager/test/test_e_stop_manager.cpp +++ b/e_stop_manager/test/test_e_stop_manager.cpp @@ -1,9 +1,9 @@ -#include -#include -#include #include -#include #include +#include +#include +#include +#include using namespace std::chrono_literals; @@ -16,276 +16,282 @@ using namespace std::chrono_literals; // Redefinition of topics and e-stops // redefined here to test if e-stop manager is able to read the config params correctly and set up the correct publishers -std::vector E_STOP_TOPICS = {"/emergency_stop_hardware", "/emergency_stop_software"}; -std::map E_STOP_NAMES_TO_TOPIC = - {{"hard_remote_e_stop", "/emergency_stop_hardware"}, - {"soft_remote_e_stop", "/emergency_stop_hardware"}, - {"big_red_button_e_stop", "/emergency_stop_software"}, - {"ui_e_stop", "/emergency_stop_software"}}; +std::vector E_STOP_TOPICS = { "/emergency_stop_hardware", "/emergency_stop_software" }; +std::map E_STOP_NAMES_TO_TOPIC = { + { "hard_remote_e_stop", "/emergency_stop_hardware" }, + { "soft_remote_e_stop", "/emergency_stop_hardware" }, + { "big_red_button_e_stop", "/emergency_stop_software" }, + { "ui_e_stop", "/emergency_stop_software" } }; template struct MsgContainer { - using MessageSharedPtr = std::shared_ptr; - - MsgContainer(const rclcpp::Node::SharedPtr &node, const std::string &topic_name) - : msg_received_(false), topic_name_(topic_name), msg_{nullptr} { - - auto callback = [this](const typename MsgType::SharedPtr msg) { - this->storeMessage(msg); - }; - subscriber_ = node->create_subscription(topic_name_, 10, callback); - } - - void storeMessage(const MessageSharedPtr &msg) { - std::lock_guard lock(mutex_); - msg_ = msg; - msg_received_ = true; - } - - MessageSharedPtr getCurrentMessage() { - std::lock_guard lock(mutex_); - return msg_; + using MessageSharedPtr = std::shared_ptr; + + MsgContainer( const rclcpp::Node::SharedPtr &node, const std::string &topic_name ) + : msg_received_( false ), topic_name_( topic_name ), msg_{ nullptr } + { + + auto callback = [this]( const typename MsgType::SharedPtr msg ) { this->storeMessage( msg ); }; + subscriber_ = node->create_subscription( topic_name_, 10, callback ); + } + + void storeMessage( const MessageSharedPtr &msg ) + { + std::lock_guard lock( mutex_ ); + msg_ = msg; + msg_received_ = true; + } + + MessageSharedPtr getCurrentMessage() + { + std::lock_guard lock( mutex_ ); + return msg_; + } + + void reset() + { + std::lock_guard lock( mutex_ ); + msg_received_ = false; + } + + // Wait for a message to be received with a specific timeout duration + bool waitForMessage( std::chrono::milliseconds timeout = std::chrono::milliseconds( 200 ), + double frequency = 10 ) + { + rclcpp::Rate rate( frequency ); + auto start_time = std::chrono::steady_clock::now(); + while ( !msg_received_ ) { + if ( std::chrono::steady_clock::now() - start_time > timeout ) { + return false; // Timeout reached + } + rate.sleep(); } - - void reset() { - std::lock_guard lock(mutex_); - msg_received_ = false; - } - - // Wait for a message to be received with a specific timeout duration - bool waitForMessage(std::chrono::milliseconds timeout = std::chrono::milliseconds(200), double frequency = 10) { - rclcpp::Rate rate(frequency); - auto start_time = std::chrono::steady_clock::now(); - while (!msg_received_) { - if (std::chrono::steady_clock::now() - start_time > timeout) { - return false; // Timeout reached - } - rate.sleep(); - } - return true; // Message received within the timeout + return true; // Message received within the timeout + } + + // Method to check if at least one publisher is subscribed to the subscriber + bool isSubscriberConnected() const + { + if ( subscriber_ ) { + auto num_publishers = subscriber_->get_publisher_count(); + return ( num_publishers > 0 ); } - - // Method to check if at least one publisher is subscribed to the subscriber - bool isSubscriberConnected() const { - if (subscriber_) { - auto num_publishers = subscriber_->get_publisher_count(); - return (num_publishers > 0); - } - return false; + return false; + } + + // Wait for a publisher to be connected with a specific frequency and timeout + bool waitForPublisher( std::chrono::milliseconds timeout = std::chrono::milliseconds( 200 ), + double frequency = 10.0 ) + { + rclcpp::Rate rate( frequency ); + auto start_time = std::chrono::steady_clock::now(); + while ( rclcpp::ok() ) { + if ( subscriber_ && subscriber_->get_publisher_count() > 0 ) { + return true; // Publisher connected + } + if ( std::chrono::steady_clock::now() - start_time > timeout ) { + return false; // Timeout reached + } + rate.sleep(); } + return false; // Unexpected failure or interruption + } - // Wait for a publisher to be connected with a specific frequency and timeout - bool waitForPublisher(std::chrono::milliseconds timeout = std::chrono::milliseconds(200), - double frequency = 10.0) { - rclcpp::Rate rate(frequency); - auto start_time = std::chrono::steady_clock::now(); - while (rclcpp::ok()) { - if (subscriber_ && subscriber_->get_publisher_count() > 0) { - return true; // Publisher connected - } - if (std::chrono::steady_clock::now() - start_time > timeout) { - return false; // Timeout reached - } - rate.sleep(); - } - return false; // Unexpected failure or interruption - } - - bool receivedMsgSinceLastReset() const { - std::lock_guard lock(mutex_); - return msg_received_; - } + bool receivedMsgSinceLastReset() const + { + std::lock_guard lock( mutex_ ); + return msg_received_; + } private: - mutable std::mutex mutex_; - MessageSharedPtr msg_; - bool msg_received_; - std::string topic_name_; - typename rclcpp::Subscription::SharedPtr subscriber_; + mutable std::mutex mutex_; + MessageSharedPtr msg_; + bool msg_received_; + std::string topic_name_; + typename rclcpp::Subscription::SharedPtr subscriber_; }; - // Mock client class for the service -class TestClient { +class TestClient +{ public: - explicit TestClient(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()) : - node_(std::make_shared("test_client", options)), - e_stop_list_msgs(MsgContainer{node_, "e_stop_list"}) { - - client_ = node_->create_client("set_e_stop"); - for (auto const &topic: E_STOP_TOPICS) { - auto sub = std::make_shared>(node_, topic); - e_stop_msgs.emplace(topic, sub); - } - - // initialize all e_stops as off (must be the same in e_stop_manager config) - for (const auto &pair: E_STOP_NAMES_TO_TOPIC) { - e_stop_state_[pair.first] = false; - } - }; - - /* - * reset Start State -> all of them should be off - */ - void resetStartStateOfEStopManager(){ - waitForConnection(); - //turn all off - for (const auto &pair: E_STOP_NAMES_TO_TOPIC) { - callEStop(pair.first, false); - } + explicit TestClient( const rclcpp::NodeOptions &options = rclcpp::NodeOptions() ) + : node_( std::make_shared( "test_client", options ) ), + e_stop_list_msgs( + MsgContainer{ node_, "e_stop_list" } ) + { + + client_ = node_->create_client( "set_e_stop" ); + for ( auto const &topic : E_STOP_TOPICS ) { + auto sub = std::make_shared>( node_, topic ); + e_stop_msgs.emplace( topic, sub ); } - bool getExpectedEStopTopicState(const std::string &topic) { - // RCLCPP_INFO_STREAM(rclcpp::get_logger("test_client"),"getExpectedEStopTopicState "< all of them should be off + */ + void resetStartStateOfEStopManager() + { + waitForConnection(); + // turn all off + for ( const auto &pair : E_STOP_NAMES_TO_TOPIC ) { callEStop( pair.first, false ); } + } + + bool getExpectedEStopTopicState( const std::string &topic ) + { + // RCLCPP_INFO_STREAM(rclcpp::get_logger("test_client"),"getExpectedEStopTopicState "<node_->get_node_base_interface(); + return state; + } + + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const + { + return this->node_->get_node_base_interface(); + } + + /** + * Waits for the publishers and service clients of the test node to connect to the e-stop manager + * -> also asserts the existence of the publishers in the e-stop-manager + * @param test_client + */ + void waitForConnection() + { + for ( const auto &pair : e_stop_msgs ) { + ASSERT_TRUE( pair.second->waitForPublisher( 300ms ) ); } - - - - /** - * Waits for the publishers and service clients of the test node to connect to the e-stop manager - * -> also asserts the existence of the publishers in the e-stop-manager - * @param test_client - */ - void waitForConnection() { - for (const auto &pair: e_stop_msgs) { - ASSERT_TRUE(pair.second->waitForPublisher(300ms)); - } - ASSERT_TRUE(client_->wait_for_service(300ms)); - } - - /** - * Reset all msg containers - */ - void resetAllMsgContainers() { - for (const auto &pair: e_stop_msgs) { - pair.second->reset(); - } - e_stop_list_msgs.reset(); + ASSERT_TRUE( client_->wait_for_service( 300ms ) ); + } + + /** + * Reset all msg containers + */ + void resetAllMsgContainers() + { + for ( const auto &pair : e_stop_msgs ) { pair.second->reset(); } + e_stop_list_msgs.reset(); + } + + /** + * Calls the e_stop service with the given values. + * @param e_stop_name + * @param requested_state + */ + void callEStop( const std::string &e_stop_name, bool requested_state, + bool invalid_e_stop_name = false ) + { + auto request = std::make_shared(); + request->name = e_stop_name; + request->value = requested_state; + auto result = client_->async_send_request( request ); + // Wait for the result. + auto status = result.wait_for( 100ms ); + EXPECT_TRUE( status == std::future_status::ready ); + if ( !invalid_e_stop_name ) { + EXPECT_TRUE( result.get()->result == e_stop_manager_msgs::srv::SetEStop::Response::SUCCESS ); + } else { + EXPECT_TRUE( result.get()->result == + e_stop_manager_msgs::srv::SetEStop::Response::INVALID_ESTOP_NAME ); } - /** - * Calls the e_stop service with the given values. - * @param e_stop_name - * @param requested_state - */ - void callEStop(const std::string &e_stop_name, bool requested_state, bool invalid_e_stop_name = false) { - auto request = std::make_shared(); - request->name = e_stop_name; - request->value = requested_state; - auto result = client_->async_send_request(request); - // Wait for the result. - auto status = result.wait_for(100ms); - EXPECT_TRUE(status == std::future_status::ready); - if (!invalid_e_stop_name){ - EXPECT_TRUE(result.get()->result == e_stop_manager_msgs::srv::SetEStop::Response::SUCCESS); - }else{ - EXPECT_TRUE(result.get()->result == e_stop_manager_msgs::srv::SetEStop::Response::INVALID_ESTOP_NAME); - - } - - e_stop_state_[e_stop_name] = requested_state; + e_stop_state_[e_stop_name] = requested_state; + } + + /** + * Waits for msg on every e_stop + e_stop_list topic and compares the received state + * with the expected e_stop_topic state. + * Assumes: that resetAllMsgContainers has been called before the e-stop request to the e-stop-manager + */ + void waitForMsgAndVerifyState() + { + // Check e_stop_topics + for ( const auto &topic : E_STOP_TOPICS ) { + EXPECT_TRUE( e_stop_msgs[topic]->waitForMessage() ); + auto msg = e_stop_msgs[topic]->getCurrentMessage(); + if ( msg ) { + auto received_msg = e_stop_msgs[topic]->receivedMsgSinceLastReset(); + RCLCPP_INFO_STREAM( + rclcpp::get_logger( "test_client" ), + "\t topic " << topic << " received " << ( received_msg ? "a" : "no" ) << " msg" + << ( received_msg ? "with value " + std::to_string( msg->data ) : "" ) + << " expected topic state is: " << getExpectedEStopTopicState( topic ) ); + // compare expected state with real state + EXPECT_EQ( getExpectedEStopTopicState( topic ), msg->data ); + } } - /** - * Waits for msg on every e_stop + e_stop_list topic and compares the received state - * with the expected e_stop_topic state. - * Assumes: that resetAllMsgContainers has been called before the e-stop request to the e-stop-manager - */ - void waitForMsgAndVerifyState(){ - // Check e_stop_topics - for (const auto &topic: E_STOP_TOPICS) { - EXPECT_TRUE(e_stop_msgs[topic]->waitForMessage()); - auto msg = e_stop_msgs[topic]->getCurrentMessage(); - if(msg){ - auto received_msg = e_stop_msgs[topic]->receivedMsgSinceLastReset(); - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_client"), - "\t topic " << topic << " received " << (received_msg ? "a" : "no") << " msg" - << (received_msg ? "with value " + std::to_string(msg->data) : "") - << " expected topic state is: " - << getExpectedEStopTopicState(topic)); - // compare expected state with real state - EXPECT_EQ(getExpectedEStopTopicState(topic), msg->data); - } - - } - - // Check e_stop_list topic - EXPECT_TRUE(e_stop_list_msgs.waitForMessage()); - auto msg = e_stop_list_msgs.getCurrentMessage(); - if (msg){ - ASSERT_EQ(msg->values.size(), msg->names.size()); - ASSERT_EQ(msg->values.size(), E_STOP_NAMES_TO_TOPIC.size()); - for(size_t i=0; ivalues.size(); i++){ - RCLCPP_INFO_STREAM(rclcpp::get_logger("test_client"), - "\t topic " << msg->names[i] - <<" value " <values[i] - << " expected topic state is: " - << e_stop_state_[msg->names[i]]); - EXPECT_EQ(msg->values[i], e_stop_state_[msg->names[i]]); - } - } + // Check e_stop_list topic + EXPECT_TRUE( e_stop_list_msgs.waitForMessage() ); + auto msg = e_stop_list_msgs.getCurrentMessage(); + if ( msg ) { + ASSERT_EQ( msg->values.size(), msg->names.size() ); + ASSERT_EQ( msg->values.size(), E_STOP_NAMES_TO_TOPIC.size() ); + for ( size_t i = 0; i < msg->values.size(); i++ ) { + RCLCPP_INFO_STREAM( rclcpp::get_logger( "test_client" ), + "\t topic " + << msg->names[i] << " value " << msg->values[i] + << " expected topic state is: " << e_stop_state_[msg->names[i]] ); + EXPECT_EQ( msg->values[i], e_stop_state_[msg->names[i]] ); + } } + } - - rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr client_; - std::map>> e_stop_msgs; - MsgContainer e_stop_list_msgs; - std::map e_stop_state_; + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr client_; + std::map>> e_stop_msgs; + MsgContainer e_stop_list_msgs; + std::map e_stop_state_; }; // Fixture class for testing EStopManager -class EStopManagerTest : public ::testing::Test { +class EStopManagerTest : public ::testing::Test +{ protected: - EStopManagerTest() : executor_(std::make_shared()) {} - - void SetUp() override { - test_client_ = std::make_shared(); - executor_ = std::make_shared(); - executor_->add_node(test_client_->get_node_base_interface()); - executor_thread_ = std::thread([this]() { this->executor_->spin(); }); - RCLCPP_INFO(rclcpp::get_logger("test_client"), "\n ************************ NEW TEST ****************** \n"); - + EStopManagerTest() + : executor_( std::make_shared() ) { } + + void SetUp() override + { + test_client_ = std::make_shared(); + executor_ = std::make_shared(); + executor_->add_node( test_client_->get_node_base_interface() ); + executor_thread_ = std::thread( [this]() { this->executor_->spin(); } ); + RCLCPP_INFO( rclcpp::get_logger( "test_client" ), + "\n ************************ NEW TEST ****************** \n" ); + } + + void TearDown() override + { + // Clean up resources if needed + test_client_.reset(); + executor_->cancel(); + std::this_thread::sleep_for( std::chrono::milliseconds( 100 ) ); + if ( executor_thread_.joinable() ) { + executor_thread_.join(); + RCLCPP_INFO( rclcpp::get_logger( "test_client" ), "Stopping executor thread!" ); } + executor_.reset(); + } - void TearDown() override { - // Clean up resources if needed - test_client_.reset(); - executor_->cancel(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - if (executor_thread_.joinable()) { - executor_thread_.join(); - RCLCPP_INFO(rclcpp::get_logger("test_client"), "Stopping executor thread!"); - - } - executor_.reset(); - } - - std::shared_ptr test_client_; - rclcpp::Executor::SharedPtr executor_; - std::thread executor_thread_; + std::shared_ptr test_client_; + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; }; - /** * Tests if the TestClient is correctly connected to the e_stop_manager */ -TEST_F(EStopManagerTest, TestConnection) { - test_client_->waitForConnection(); -} +TEST_F( EStopManagerTest, TestConnection ) { test_client_->waitForConnection(); } /** * Client request via the service client that an e_stop is on @@ -293,95 +299,103 @@ TEST_F(EStopManagerTest, TestConnection) { * -> e_stop_list must be published * -> test starts with all e_stops off and iterativley turns every e_stop on then off again */ -TEST_F(EStopManagerTest, ServiceRequestChangesTopics) { - test_client_->resetStartStateOfEStopManager(); - // iterate all available e-stops - for (const auto &pair: E_STOP_NAMES_TO_TOPIC) { - test_client_->resetAllMsgContainers(); - // request 1 e-stop to be active - test_client_->callEStop(pair.first, true); - test_client_->waitForMsgAndVerifyState(); - - // turn Off - test_client_->resetAllMsgContainers(); - // request 1 e-stop to be active - test_client_->callEStop(pair.first, false); - test_client_->waitForMsgAndVerifyState(); - } +TEST_F( EStopManagerTest, ServiceRequestChangesTopics ) +{ + test_client_->resetStartStateOfEStopManager(); + // iterate all available e-stops + for ( const auto &pair : E_STOP_NAMES_TO_TOPIC ) { + test_client_->resetAllMsgContainers(); + // request 1 e-stop to be active + test_client_->callEStop( pair.first, true ); + test_client_->waitForMsgAndVerifyState(); + + // turn Off + test_client_->resetAllMsgContainers(); + // request 1 e-stop to be active + test_client_->callEStop( pair.first, false ); + test_client_->waitForMsgAndVerifyState(); + } } /** * Client request via the service client that an e_stop is off, but is already off * -> should stay that way */ -TEST_F(EStopManagerTest, ServiceRequesteStopAlreadyOff) { - test_client_->resetStartStateOfEStopManager(); - // iterate all available e-stops - for (const auto &pair: E_STOP_NAMES_TO_TOPIC) { - test_client_->resetAllMsgContainers(); - test_client_->callEStop(pair.first, false); - EXPECT_FALSE(test_client_->e_stop_list_msgs.waitForMessage(300ms)); - for(const auto& topic: E_STOP_TOPICS){ - EXPECT_FALSE(test_client_->e_stop_msgs[topic]->waitForMessage(1ms));//already waited long enough above - } +TEST_F( EStopManagerTest, ServiceRequesteStopAlreadyOff ) +{ + test_client_->resetStartStateOfEStopManager(); + // iterate all available e-stops + for ( const auto &pair : E_STOP_NAMES_TO_TOPIC ) { + test_client_->resetAllMsgContainers(); + test_client_->callEStop( pair.first, false ); + EXPECT_FALSE( test_client_->e_stop_list_msgs.waitForMessage( 300ms ) ); + for ( const auto &topic : E_STOP_TOPICS ) { + EXPECT_FALSE( test_client_->e_stop_msgs[topic]->waitForMessage( + 1ms ) ); // already waited long enough above } + } } /** * Client request via the service client that an e_stop is on, but is already on * -> should stay that way */ -TEST_F(EStopManagerTest, ServiceRequesteStopAlreadyOn) { - test_client_->resetStartStateOfEStopManager(); - // Turn all on - for (const auto &pair: E_STOP_NAMES_TO_TOPIC) { - test_client_->resetAllMsgContainers(); - // request 1 e-stop to be active - test_client_->callEStop(pair.first, true); - test_client_->waitForMsgAndVerifyState(); - } - // turn on again -> should remain on - for (const auto &pair: E_STOP_NAMES_TO_TOPIC) { - test_client_->resetAllMsgContainers(); - test_client_->callEStop(pair.first, true ); - - EXPECT_FALSE(test_client_->e_stop_list_msgs.waitForMessage(300ms)); - for(const auto& topic: E_STOP_TOPICS){ - EXPECT_FALSE(test_client_->e_stop_msgs[topic]->waitForMessage(10ms));//already waited long enough above - } +TEST_F( EStopManagerTest, ServiceRequesteStopAlreadyOn ) +{ + test_client_->resetStartStateOfEStopManager(); + // Turn all on + for ( const auto &pair : E_STOP_NAMES_TO_TOPIC ) { + test_client_->resetAllMsgContainers(); + // request 1 e-stop to be active + test_client_->callEStop( pair.first, true ); + test_client_->waitForMsgAndVerifyState(); + } + // turn on again -> should remain on + for ( const auto &pair : E_STOP_NAMES_TO_TOPIC ) { + test_client_->resetAllMsgContainers(); + test_client_->callEStop( pair.first, true ); + + EXPECT_FALSE( test_client_->e_stop_list_msgs.waitForMessage( 300ms ) ); + for ( const auto &topic : E_STOP_TOPICS ) { + EXPECT_FALSE( test_client_->e_stop_msgs[topic]->waitForMessage( + 10ms ) ); // already waited long enough above } + } } /** * Call service with non_exising e_stop name */ -TEST_F(EStopManagerTest, NonExistingEStopName) { - test_client_->resetStartStateOfEStopManager(); - - // turn 1 e_stop on - test_client_->resetAllMsgContainers(); - test_client_->callEStop(E_STOP_NAMES_TO_TOPIC.begin()->first, true); - test_client_->waitForMsgAndVerifyState(); - - test_client_->resetAllMsgContainers(); - // turn on non-exising e_stop - test_client_->callEStop("DOES_NOT_EXIST", true, true); - // test that no msg arrives -> e_stop remains correct - EXPECT_FALSE(test_client_->e_stop_list_msgs.waitForMessage(300ms)); - for(const auto& topic: E_STOP_TOPICS){ - EXPECT_FALSE(test_client_->e_stop_msgs[topic]->waitForMessage(10ms));//already waited long enough above - } - - // turn 1 e_stop off - test_client_->callEStop(E_STOP_NAMES_TO_TOPIC.begin()->first, false); - test_client_->waitForMsgAndVerifyState(); +TEST_F( EStopManagerTest, NonExistingEStopName ) +{ + test_client_->resetStartStateOfEStopManager(); + + // turn 1 e_stop on + test_client_->resetAllMsgContainers(); + test_client_->callEStop( E_STOP_NAMES_TO_TOPIC.begin()->first, true ); + test_client_->waitForMsgAndVerifyState(); + + test_client_->resetAllMsgContainers(); + // turn on non-exising e_stop + test_client_->callEStop( "DOES_NOT_EXIST", true, true ); + // test that no msg arrives -> e_stop remains correct + EXPECT_FALSE( test_client_->e_stop_list_msgs.waitForMessage( 300ms ) ); + for ( const auto &topic : E_STOP_TOPICS ) { + EXPECT_FALSE( test_client_->e_stop_msgs[topic]->waitForMessage( + 10ms ) ); // already waited long enough above + } + + // turn 1 e_stop off + test_client_->callEStop( E_STOP_NAMES_TO_TOPIC.begin()->first, false ); + test_client_->waitForMsgAndVerifyState(); } -int main(int argc, char **argv) { - ::testing::InitGoogleTest(&argc, argv); - rclcpp::init(argc, argv); +int main( int argc, char **argv ) +{ + ::testing::InitGoogleTest( &argc, argv ); + rclcpp::init( argc, argv ); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; } \ No newline at end of file diff --git a/e_stop_manager_msgs/package.xml b/e_stop_manager_msgs/package.xml index 4af5c21..f35d34f 100644 --- a/e_stop_manager_msgs/package.xml +++ b/e_stop_manager_msgs/package.xml @@ -4,11 +4,10 @@ 0.0.0 The e_stop_manager_msgs package - Katrin Becker + Aljoscha Schmidt TODO - ament_cmake