From 3a2e9a9fddf3ca46359fec5b5cc9a857224cdfff Mon Sep 17 00:00:00 2001 From: fbrand-new Date: Fri, 15 Dec 2023 16:56:18 +0000 Subject: [PATCH 1/4] Add dev conf directories to gitignore --- .gitignore | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 09eb27a..3bc14c1 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,6 @@ /compile_commands.json /.clangd/ CMakeLists.txt.user* -/ros2_example/ \ No newline at end of file +/ros2_example/ +.vscode +.devcontainer \ No newline at end of file From 8e86da2dc2cfc911874fa6b415dad51fc40330ab Mon Sep 17 00:00:00 2001 From: fbrand-new Date: Tue, 6 Feb 2024 21:51:30 +0000 Subject: [PATCH 2/4] Check network skill. Start/stop service skill --- .gitignore | 7 +- README.md | 8 +- ros2prova/BT/simple_bt.xml | 9 +- ros2prova/check_network_skill/CMakeLists.txt | 54 +++++++ .../include/CheckNetworkDataModel.h | 81 +++++++++++ .../include/CheckNetworkSkill.h | 42 ++++++ ros2prova/check_network_skill/package.xml | 24 ++++ .../src/CheckNetworkDataModel.cpp | 136 ++++++++++++++++++ .../src/CheckNetworkSkill.cpp | 116 +++++++++++++++ .../src/CheckNetworkSkillSM.scxml | 33 +++++ ros2prova/check_network_skill/src/main.cpp | 22 +++ ros2prova/cyclone_dds_settings.xml | 4 +- ros2prova/launch/bt_launch.py | 56 +++++--- ros2prova/launch/bt_launch_network.py | 52 +++++++ ros2prova/start_services_skill/CMakeLists.txt | 54 +++++++ .../include/StartServicesDataModel.h | 43 ++++++ .../include/StartServicesSkill.h | 42 ++++++ ros2prova/start_services_skill/package.xml | 27 ++++ .../src/StartServicesDataModel.cpp | 116 +++++++++++++++ .../src/StartServicesSkill.cpp | 98 +++++++++++++ .../src/StartServicesSkillSM.scxml | 22 +++ ros2prova/start_services_skill/src/main.cpp | 25 ++++ ros2prova/stop_services_skill/CMakeLists.txt | 54 +++++++ .../include/StopServicesDataModel.h | 43 ++++++ .../include/StopServicesSkill.h | 42 ++++++ ros2prova/stop_services_skill/package.xml | 27 ++++ .../src/StopServicesDataModel.cpp | 116 +++++++++++++++ .../src/StopServicesSkill.cpp | 98 +++++++++++++ .../src/StopServicesSkillSM.scxml | 22 +++ ros2prova/stop_services_skill/src/main.cpp | 25 ++++ 30 files changed, 1469 insertions(+), 29 deletions(-) create mode 100644 ros2prova/check_network_skill/CMakeLists.txt create mode 100644 ros2prova/check_network_skill/include/CheckNetworkDataModel.h create mode 100644 ros2prova/check_network_skill/include/CheckNetworkSkill.h create mode 100644 ros2prova/check_network_skill/package.xml create mode 100644 ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp create mode 100644 ros2prova/check_network_skill/src/CheckNetworkSkill.cpp create mode 100644 ros2prova/check_network_skill/src/CheckNetworkSkillSM.scxml create mode 100644 ros2prova/check_network_skill/src/main.cpp create mode 100644 ros2prova/launch/bt_launch_network.py create mode 100644 ros2prova/start_services_skill/CMakeLists.txt create mode 100644 ros2prova/start_services_skill/include/StartServicesDataModel.h create mode 100644 ros2prova/start_services_skill/include/StartServicesSkill.h create mode 100644 ros2prova/start_services_skill/package.xml create mode 100644 ros2prova/start_services_skill/src/StartServicesDataModel.cpp create mode 100644 ros2prova/start_services_skill/src/StartServicesSkill.cpp create mode 100644 ros2prova/start_services_skill/src/StartServicesSkillSM.scxml create mode 100644 ros2prova/start_services_skill/src/main.cpp create mode 100644 ros2prova/stop_services_skill/CMakeLists.txt create mode 100644 ros2prova/stop_services_skill/include/StopServicesDataModel.h create mode 100644 ros2prova/stop_services_skill/include/StopServicesSkill.h create mode 100644 ros2prova/stop_services_skill/package.xml create mode 100644 ros2prova/stop_services_skill/src/StopServicesDataModel.cpp create mode 100644 ros2prova/stop_services_skill/src/StopServicesSkill.cpp create mode 100644 ros2prova/stop_services_skill/src/StopServicesSkillSM.scxml create mode 100644 ros2prova/stop_services_skill/src/main.cpp diff --git a/.gitignore b/.gitignore index 3bc14c1..f761139 100644 --- a/.gitignore +++ b/.gitignore @@ -8,4 +8,9 @@ CMakeLists.txt.user* /ros2_example/ .vscode -.devcontainer \ No newline at end of file +.devcontainer +ros2prova/**/build +ros2prova/**/install +ros2prova/**/log +ros2prova/lifecycle_py/* +ros2prova/stop_start_component \ No newline at end of file diff --git a/README.md b/README.md index bec6df1..4c54675 100644 --- a/README.md +++ b/README.md @@ -1 +1,7 @@ -# Convince_simple_bts \ No newline at end of file +# Convince_simple_bts + +## TODO +- Test connection lost between two machines. +- Make it sudoless. +- Modularly define the program to start and stop at runtime. [Right now it is a single program. Make it more than one] +- Turn Misael's code into a lifecycle node. \ No newline at end of file diff --git a/ros2prova/BT/simple_bt.xml b/ros2prova/BT/simple_bt.xml index 017bfef..e212481 100644 --- a/ros2prova/BT/simple_bt.xml +++ b/ros2prova/BT/simple_bt.xml @@ -3,11 +3,14 @@ - + + + + diff --git a/ros2prova/check_network_skill/CMakeLists.txt b/ros2prova/check_network_skill/CMakeLists.txt new file mode 100644 index 0000000..0d88887 --- /dev/null +++ b/ros2prova/check_network_skill/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.16) +project(check_network_skill) +# set(CMAKE_CXX_STANDARD 20) +# set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(bt_interfaces REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(Qt6 COMPONENTS Core Scxml StateMachine REQUIRED) +add_executable(${PROJECT_NAME} ) + +if (NOT Qt6_FOUND) + message("qt6 not found") +endif() + +# find dependencies +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +ament_target_dependencies(${PROJECT_NAME} bt_interfaces sensor_msgs rclcpp) +target_link_libraries(${PROJECT_NAME} Qt6::Core Qt6::Scxml Qt6::StateMachine) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) +target_sources( ${PROJECT_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/CheckNetworkSkill.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/CheckNetworkSkill.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/CheckNetworkDataModel.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/CheckNetworkDataModel.h) + + +install(TARGETS ${PROJECT_NAME} +DESTINATION lib/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +qt6_add_statecharts(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/src/CheckNetworkSkillSM.scxml) + +ament_package() diff --git a/ros2prova/check_network_skill/include/CheckNetworkDataModel.h b/ros2prova/check_network_skill/include/CheckNetworkDataModel.h new file mode 100644 index 0000000..a7bfa91 --- /dev/null +++ b/ros2prova/check_network_skill/include/CheckNetworkDataModel.h @@ -0,0 +1,81 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#ifndef __CHECKNETWORKDATAMODEL_H_ +#define __CHECKNETWORKDATAMODEL_H_ + +#include + +#include +#include +#include +#include +#include + +// Ping utility structs +#include +#include +#include +#include +#include +#include + +// Packet struct +constexpr int PING_PKT_S = 64; + +struct ping_pkt { + struct icmphdr hdr; + char msg[PING_PKT_S - sizeof(struct icmphdr)]; +}; + +class CheckNetworkDataModel: public QScxmlCppDataModel +{ + Q_SCXML_DATAMODEL + +public: + CheckNetworkDataModel() = default; + void set_name(std::string name); + bool close(); + void spin(); + void topic_callback(); + bool start() ; + bool setup(const QVariantMap& initialDataValues) override; +private: + std::string m_name = "CheckNetworkDataModel"; + int m_msg_count = 0; + bool m_is_connected = true; + std::shared_ptr m_thread; + rclcpp::TimerBase::SharedPtr timer_; + + std::string m_address_name; + std::shared_ptr m_node; + + //host can be either the name of the resource (e.g. r1-base) or its ip addr. + bool isNetworkConnected(const std::string& host); + + // Calculating the Check Sum + // TODO: understand what is this + unsigned short checksum(void* b, int len) + { + unsigned short* buf = static_cast(b); + unsigned int sum = 0; + unsigned short result; + + for (sum = 0; len > 1; len -= 2) + sum += *buf++; + if (len == 1) + sum += *(unsigned char*)buf; + sum = (sum > 16) + (sum & 0xFFFF); + sum += (sum > 16); + result = ~sum; + return result; + } +}; + +Q_DECLARE_METATYPE(::CheckNetworkDataModel*) + +#endif \ No newline at end of file diff --git a/ros2prova/check_network_skill/include/CheckNetworkSkill.h b/ros2prova/check_network_skill/include/CheckNetworkSkill.h new file mode 100644 index 0000000..97131f0 --- /dev/null +++ b/ros2prova/check_network_skill/include/CheckNetworkSkill.h @@ -0,0 +1,42 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +# pragma once + +#include +#include +#include "CheckNetworkSkillSM.h" +#include "CheckNetworkDataModel.h" +#include +#include +#include +#include + +class CheckNetworkSkill +{ +public: + CheckNetworkSkill(std::string name ); + + bool start(int argc, char * argv[]); + static void spin(std::shared_ptr node); + void request_ack( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response); + void send_start( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + void send_stop([[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + +private: + std::shared_ptr m_threadSpin; + std::shared_ptr m_node; + rclcpp::Service::SharedPtr m_requestAckService; + rclcpp::Service::SharedPtr m_sendStartService; + rclcpp::Service::SharedPtr m_sendStopService; + std::string m_name; + CheckNetworkDataModel m_dataModel; + CheckNetworkSkillSM m_stateMachine; +}; diff --git a/ros2prova/check_network_skill/package.xml b/ros2prova/check_network_skill/package.xml new file mode 100644 index 0000000..d3ef8ac --- /dev/null +++ b/ros2prova/check_network_skill/package.xml @@ -0,0 +1,24 @@ + + + + check_network_skill + 0.0.0 + TODO: Package description + Stefano Bernagozzi + TODO: License declaration + + ament_cmake + bt_interfaces + sensor_msgs + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + rosidl_default_runtime + + rosidl_default_generators + + + ament_cmake + + diff --git a/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp b/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp new file mode 100644 index 0000000..59dfa01 --- /dev/null +++ b/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp @@ -0,0 +1,136 @@ +/****************************************************************************** + * * + * Copyright (C) 2024 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#include "CheckNetworkDataModel.h" + +#include +#include +#include +#include + +// Includes to perform ping +#include +#include + +using namespace std::chrono_literals; + + +void CheckNetworkDataModel::set_name(std::string name) +{ + m_name=name; +} + +bool CheckNetworkDataModel::setup(const QVariantMap &initialDataValues) +{ + Q_UNUSED(initialDataValues) + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); + } + + m_node = rclcpp::Node::make_shared(m_name); + m_address_name = "127.0.0.1"; + + timer_ = m_node->create_wall_timer(1000ms, std::bind(&CheckNetworkDataModel::topic_callback, this)); + + RCLCPP_DEBUG(m_node->get_logger(), "CheckNetworkDataModel::start"); + m_thread = std::make_shared([this]{ start();}); + return true; +} + + +bool CheckNetworkDataModel::close() +{ + rclcpp::shutdown(); + return true; +} + +void CheckNetworkDataModel::spin() +{ + rclcpp::spin(m_node); +} + +bool CheckNetworkDataModel::start() { + std::cout << "Before spinning" << std::endl; + spin(); + std::cout << "Finished spinning" << std::endl; + close(); + return true; +} + +void CheckNetworkDataModel::topic_callback() { + std::cout << "CALLBACK" << std::endl; + m_is_connected = isNetworkConnected(m_address_name); + std::cout << "Our machine is connected? " << m_is_connected; +} + +bool CheckNetworkDataModel::isNetworkConnected(const std::string& host) { + + struct addrinfo hints, *res; + struct sockaddr_in r_addr; + socklen_t r_addr_len; + int sockfd; + bool is_connected = false; + + // Setup hints + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_INET; + hints.ai_socktype = SOCK_RAW; + hints.ai_protocol = IPPROTO_ICMP; + + // Get address info -> this will take care of the dns lookup if needed + if (getaddrinfo(host.c_str(), NULL, &hints, &res) != 0) { + return false; + } + + // Create socket + // sockfd = socket(res->ai_family, res->ai_socktype, res->ai_protocol); + sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP); + if (sockfd == -1) { + std::cout << "Failed to create socket" << std::endl; + freeaddrinfo(res); + return false; + } + + // Build a packet to send to host + struct ping_pkt pckt; + memset(&pckt,0,sizeof(pckt)); + pckt.hdr.type = ICMP_ECHO; + pckt.hdr.un.echo.id = getpid(); + for (std::size_t i = 0; iai_addr,res->ai_addrlen); + if(bytes_sent == -1) { + std::cout << "Failed to send packet address" << std::endl; + freeaddrinfo(res); + return false; + } + + // Receive a packet + char rbuffer[128]; + int bytes_received = recvfrom(sockfd,rbuffer,sizeof(rbuffer),0, + (struct sockaddr*) &r_addr, &r_addr_len); + if(bytes_received == -1) { + std::cout << "Failed to receive packets" << std::endl; + is_connected = false; + } + + if(is_connected){ + std::cout << "is connected!" << std::endl; + } + + freeaddrinfo(res); + + return is_connected; +} diff --git a/ros2prova/check_network_skill/src/CheckNetworkSkill.cpp b/ros2prova/check_network_skill/src/CheckNetworkSkill.cpp new file mode 100644 index 0000000..d6e07bc --- /dev/null +++ b/ros2prova/check_network_skill/src/CheckNetworkSkill.cpp @@ -0,0 +1,116 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#include "CheckNetworkSkill.h" + +#include +#include +#include +#include +#include + +CheckNetworkSkill::CheckNetworkSkill(std::string name ) : + m_name(std::move(name)) +{ + m_dataModel.set_name(name+"dataModel"); + m_stateMachine.setDataModel(&m_dataModel); +} + + +void CheckNetworkSkill::spin(std::shared_ptr node) +{ + rclcpp::spin(node); + rclcpp::shutdown(); +} + + +bool CheckNetworkSkill::start(int argc, char*argv[]) +{ + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ argc, /*argv*/ argv); + } + + m_node = rclcpp::Node::make_shared(m_name); + + + RCLCPP_DEBUG(m_node->get_logger(), "CheckNetworkSkill::start"); + std::cout << "CheckNetworkSkill::start"; + m_requestAckService = m_node->create_service("/CheckNetworkSkill/RequestAck", + std::bind(&CheckNetworkSkill::request_ack, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_sendStartService = m_node->create_service("/CheckNetworkSkill/SendStart", + std::bind(&CheckNetworkSkill::send_start, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_sendStopService = m_node->create_service("/CheckNetworkSkill/SendStop", + std::bind(&CheckNetworkSkill::send_stop, + this, + std::placeholders::_1, + std::placeholders::_2)); + + m_stateMachine.start(); + m_threadSpin = std::make_shared(spin, m_node); + return true; +} + +void CheckNetworkSkill::request_ack( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_DEBUG(m_node->get_logger(), "CheckNetworkSkill::request_ack"); + std::cout << "CheckNetworkSkill::request_ack\n"; + + auto message = bt_interfaces::msg::RequestAck(); + for (const auto& state : m_stateMachine.activeStateNames()) { + if (state == "idle") { + std::cout << "STATE:IDLE" << std::endl; + m_stateMachine.submitEvent("CMD_START"); + response->status.status = message.SKILL_IDLE; + response->is_ok = true; + } + if (state == "get") { + std::cout << "STATE:GET" << std::endl; + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_IDLE; + response->is_ok = true; + } + if (state == "success") { + std::cout << "STATE:SUC" << std::endl; + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_SUCCESS; + response->is_ok = true; + } + if (state == "failure") { + std::cout << "STATE:FAIL" << std::endl; + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_FAILURE; + response->is_ok = true; + } + } +} +void CheckNetworkSkill::send_start( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + RCLCPP_DEBUG(m_node->get_logger(), "CheckNetworkSkill::send_start"); + std::cout << "CheckNetworkSkill::send_start"; + m_stateMachine.submitEvent("CMD_START"); + response->is_ok = true; +} + +void CheckNetworkSkill::send_stop( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + + RCLCPP_DEBUG(m_node->get_logger(), "CheckNetworkSkill::send_stop"); + std::cout << "CheckNetworkSkill::send_stop"; + m_stateMachine.submitEvent("CMD_STOP", QStateMachine::HighPriority); + response->is_ok = true; +} diff --git a/ros2prova/check_network_skill/src/CheckNetworkSkillSM.scxml b/ros2prova/check_network_skill/src/CheckNetworkSkillSM.scxml new file mode 100644 index 0000000..df2f725 --- /dev/null +++ b/ros2prova/check_network_skill/src/CheckNetworkSkillSM.scxml @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2prova/check_network_skill/src/main.cpp b/ros2prova/check_network_skill/src/main.cpp new file mode 100644 index 0000000..0bd8f1b --- /dev/null +++ b/ros2prova/check_network_skill/src/main.cpp @@ -0,0 +1,22 @@ +#include +#include +#include + +#include +#include "CheckNetworkSkill.h" + +#include +#include + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + CheckNetworkSkill stateMachine("CheckNetwork"); + stateMachine.start(argc, argv); + + int ret=app.exec(); + + return ret; + +} + diff --git a/ros2prova/cyclone_dds_settings.xml b/ros2prova/cyclone_dds_settings.xml index 4d738b7..1d68f8e 100644 --- a/ros2prova/cyclone_dds_settings.xml +++ b/ros2prova/cyclone_dds_settings.xml @@ -3,7 +3,7 @@ false - + @@ -11,7 +11,7 @@ auto - + diff --git a/ros2prova/launch/bt_launch.py b/ros2prova/launch/bt_launch.py index 930ffd7..9942e59 100644 --- a/ros2prova/launch/bt_launch.py +++ b/ros2prova/launch/bt_launch.py @@ -5,36 +5,48 @@ def generate_launch_description(): return LaunchDescription([ Node( - package='alarm_battery_low_skill', - executable='alarm_battery_low_skill', + package='stop_services_skill', + executable='stop_services_skill', output='screen', - arguments=['--ros-args', '--log-level', 'debug'] + arguments=['--ros-args','--log-level', 'debug'] ), Node( - package='battery_drainer_skill', - executable='battery_drainer_skill', + package='start_services_skill', + executable='start_services_skill', output='screen', - arguments=['--ros-args', '--log-level', 'debug'] - ), - Node( - package='battery_drainer_component', - executable='battery_drainer_component', - arguments=[('__log_level:=debug')] - ), - Node( - package='battery_level_skill', - executable='battery_level_skill', - arguments=[('__log_level:=debug')] - ), - Node( - package='battery_drainer_component', - executable='battery_drainer_component', - arguments=[('__log_level:=debug')] + arguments=['--ros-args','--log-level', 'debug'] ), + # Node( + # package='alarm_battery_low_skill', + # executable='alarm_battery_low_skill', + # output='screen', + # arguments=['--ros-args', '--log-level', 'debug'] + # ), + # Node( + # package='battery_drainer_skill', + # executable='battery_drainer_skill', + # output='screen', + # arguments=['--ros-args', '--log-level', 'debug'] + # ), + # Node( + # package='battery_drainer_component', + # executable='battery_drainer_component', + # arguments=[('__log_level:=debug')] + # ), + # Node( + # package='battery_level_skill', + # executable='battery_level_skill', + # arguments=[('__log_level:=debug')] + # ), + # Node( + # package='battery_drainer_component', + # executable='battery_drainer_component', + # arguments=[('__log_level:=debug')] + # ), Node( package='bt_executable', executable='bt_executable', - arguments=[ '/home/user1/convince_simple_bts/ros2prova/BT/simple_bt.xml',('__log_level:=debug')] + arguments=[ '/workspaces/convince_simple_bts/ros2prova/BT/simple_bt.xml',('__log_level:=debug')] ) ]) diff --git a/ros2prova/launch/bt_launch_network.py b/ros2prova/launch/bt_launch_network.py new file mode 100644 index 0000000..7ef290f --- /dev/null +++ b/ros2prova/launch/bt_launch_network.py @@ -0,0 +1,52 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='check_network_skill', + executable='check_network_skill', + output='screen', + arguments=['--ros-args','--log-level', 'debug'] + ), + # Node( + # package='start_services_skill', + # executable='start_services_skill', + # output='screen', + # arguments=['--ros-args','--log-level', 'debug'] + # ), + # Node( + # package='alarm_battery_low_skill', + # executable='alarm_battery_low_skill', + # output='screen', + # arguments=['--ros-args', '--log-level', 'debug'] + # ), + # Node( + # package='battery_drainer_skill', + # executable='battery_drainer_skill', + # output='screen', + # arguments=['--ros-args', '--log-level', 'debug'] + # ), + # Node( + # package='battery_drainer_component', + # executable='battery_drainer_component', + # arguments=[('__log_level:=debug')] + # ), + # Node( + # package='battery_level_skill', + # executable='battery_level_skill', + # arguments=[('__log_level:=debug')] + # ), + # Node( + # package='battery_drainer_component', + # executable='battery_drainer_component', + # arguments=[('__log_level:=debug')] + # ), + Node( + package='bt_executable', + executable='bt_executable', + arguments=[ '/workspaces/convince_simple_bts/ros2prova/BT/simple_bt.xml',('__log_level:=debug')] + ) + ]) + diff --git a/ros2prova/start_services_skill/CMakeLists.txt b/ros2prova/start_services_skill/CMakeLists.txt new file mode 100644 index 0000000..690ba03 --- /dev/null +++ b/ros2prova/start_services_skill/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(start_services_skill) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(bt_interfaces REQUIRED) +find_package(other_interfaces REQUIRED) +find_package(Qt6 COMPONENTS Core Scxml StateMachine REQUIRED) +add_executable(${PROJECT_NAME} ) + +if (NOT Qt6_FOUND) + message("qt6 not found") +endif() + +# find dependencies +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +ament_target_dependencies(${PROJECT_NAME} bt_interfaces other_interfaces rclcpp rclcpp_lifecycle lifecycle_msgs) +target_link_libraries(${PROJECT_NAME} Qt6::Core Qt6::Scxml Qt6::StateMachine) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) +target_sources( ${PROJECT_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/StartServicesSkill.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/StartServicesSkill.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/StartServicesDataModel.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/StartServicesDataModel.h) + + +install(TARGETS ${PROJECT_NAME} +DESTINATION lib/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +qt6_add_statecharts(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/src/StartServicesSkillSM.scxml) + +ament_package() diff --git a/ros2prova/start_services_skill/include/StartServicesDataModel.h b/ros2prova/start_services_skill/include/StartServicesDataModel.h new file mode 100644 index 0000000..0d94cf0 --- /dev/null +++ b/ros2prova/start_services_skill/include/StartServicesDataModel.h @@ -0,0 +1,43 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include + +using namespace std::chrono_literals; + +class StartServicesDataModel: public QScxmlCppDataModel +{ + Q_SCXML_DATAMODEL + +public: + StartServicesDataModel() = default; + bool start(); + void set_name(std::string name); + bool setup(const QVariantMap& initialDataValues) override; +private: + std::shared_ptr m_node; + rclcpp::Client::SharedPtr m_client_get_state; + rclcpp::Client::SharedPtr m_client_change_state; + std::string m_name = "StartServicesDataModel"; + std::chrono::seconds m_timeout = 3s; + + bool is_client_available(const std::chrono::seconds& timeout = 3s); + std::optional get_state(const std::chrono::seconds& timeout = 3s); +}; + +Q_DECLARE_METATYPE(::StartServicesDataModel*) diff --git a/ros2prova/start_services_skill/include/StartServicesSkill.h b/ros2prova/start_services_skill/include/StartServicesSkill.h new file mode 100644 index 0000000..58ffad4 --- /dev/null +++ b/ros2prova/start_services_skill/include/StartServicesSkill.h @@ -0,0 +1,42 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +# pragma once + +#include +#include +#include "StartServicesSkillSM.h" +#include "StartServicesDataModel.h" +#include +#include +#include +#include + +class StartServicesSkill +{ +public: + StartServicesSkill(std::string name ); + + bool start(int argc, char * argv[]); + static void spin(std::shared_ptr node); + void request_ack( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response); + void send_start( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + void send_stop([[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + +private: + std::shared_ptr m_threadSpin; + std::shared_ptr m_node; + rclcpp::Service::SharedPtr m_requestAckService; + rclcpp::Service::SharedPtr m_sendStartService; + rclcpp::Service::SharedPtr m_sendStopService; + std::string m_name; + StartServicesDataModel m_dataModel; + StartServicesSkillSM m_stateMachine; +}; diff --git a/ros2prova/start_services_skill/package.xml b/ros2prova/start_services_skill/package.xml new file mode 100644 index 0000000..44a4e38 --- /dev/null +++ b/ros2prova/start_services_skill/package.xml @@ -0,0 +1,27 @@ + + + + start_services_skill + 0.0.0 + A BT skill that stops the provided service + Stefano Bernagozzi + Apache-2.0 + + ament_cmake + bt_interfaces + other_interfaces + rclcpp + lifecycle_msgs + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + rosidl_default_runtime + + rosidl_default_generators + + + ament_cmake + + diff --git a/ros2prova/start_services_skill/src/StartServicesDataModel.cpp b/ros2prova/start_services_skill/src/StartServicesDataModel.cpp new file mode 100644 index 0000000..188e5c5 --- /dev/null +++ b/ros2prova/start_services_skill/src/StartServicesDataModel.cpp @@ -0,0 +1,116 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ +#include "StartServicesDataModel.h" + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include +#include +#include + + + +void StartServicesDataModel::set_name(std::string name) +{ + m_name=name; +} + +bool StartServicesDataModel::setup(const QVariantMap &initialDataValues) +{ + Q_UNUSED(initialDataValues) + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); + } + + m_node = rclcpp::Node::make_shared(m_name); + m_client_get_state = m_node->create_client("/lc_talker/get_state"); + m_client_change_state = m_node->create_client("/lc_talker/change_state"); + + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesDataModel::start"); + std::cout << "StartServicesDataModel::start"; + + return true; +} + +bool StartServicesDataModel::is_client_available(const std::chrono::seconds& timeout) { + + // Check if client is available, else return + bool is_client_available = m_client_get_state->wait_for_service(timeout); + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return false; + } + if(!is_client_available){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, aborting."); + return false; + } + + return true; +} + +std::optional StartServicesDataModel::get_state(const std::chrono::seconds& timeout) { + + if(!is_client_available(timeout)) { + return std::nullopt; + } + + // Get the state of the managed node + auto request = std::make_shared(); + auto result = m_client_get_state->async_send_request(request); + + if (rclcpp::spin_until_future_complete(m_node, result,timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesDataModel::get_state(). Success"); + return result.get()->current_state; + } else { + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesDataModel::get_state(). Failure"); + return std::nullopt; + } + +} + +bool StartServicesDataModel::start() { + + if(!is_client_available()){ + return false; + } + + // Check if managed node is in state active and make it inactive + // Check https://github.com/ros2/rcl_interfaces/blob/humble/lifecycle_msgs/msg/State.msg for reference + std::optional current_state = this->get_state(); + if(!current_state) { + return false; + } + + if(current_state.value().id != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(m_node->get_logger(), "Cannot start service since it is not inactive"); + return false; + } + else { + + // Change the state of the managed node to inactive + // Check https://github.com/ros2/rcl_interfaces/blob/humble/lifecycle_msgs/msg/Transition.msg for reference + auto request = std::make_shared(); + request->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE; + + auto result = m_client_change_state->async_send_request(request); + + if (rclcpp::spin_until_future_complete(m_node,result,m_timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesDataModel::start(). Success"); + return true; + } else { + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesDataModel::start(). Failure"); + return false; + } + } + + return true; +} \ No newline at end of file diff --git a/ros2prova/start_services_skill/src/StartServicesSkill.cpp b/ros2prova/start_services_skill/src/StartServicesSkill.cpp new file mode 100644 index 0000000..fa67219 --- /dev/null +++ b/ros2prova/start_services_skill/src/StartServicesSkill.cpp @@ -0,0 +1,98 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#include "StartServicesSkill.h" + +#include +#include +#include +#include +#include + +StartServicesSkill::StartServicesSkill(std::string name ) : + m_name(std::move(name)) +{ + m_dataModel.set_name(name+"dataModel"); + m_stateMachine.setDataModel(&m_dataModel); +} + + +void StartServicesSkill::spin(std::shared_ptr node) +{ + rclcpp::spin(node); + rclcpp::shutdown(); +} + + +bool StartServicesSkill::start(int argc, char*argv[]) +{ + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ argc, /*argv*/ argv); + } + m_node = rclcpp::Node::make_shared(m_name); + + + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesSkill::start"); + std::cout << "StartServicesSkill::start"; + m_requestAckService = m_node->create_service("/StartServicesSkill/RequestAck", + std::bind(&StartServicesSkill::request_ack, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_sendStartService = m_node->create_service("/StartServicesSkill/SendStart", + std::bind(&StartServicesSkill::send_start, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_sendStopService = m_node->create_service("/StartServicesSkill/SendStop", + std::bind(&StartServicesSkill::send_stop, + this, + std::placeholders::_1, + std::placeholders::_2)); + + m_stateMachine.start(); + m_threadSpin = std::make_shared(spin, m_node); + return true; +} + +void StartServicesSkill::request_ack( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response) +{ + auto message = bt_interfaces::msg::RequestAck(); + for (const auto& state : m_stateMachine.activeStateNames()) { + if (state == "start") { + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_RUNNING; + } + if (state == "idle") { + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_IDLE; + } + } + response->is_ok = true; +} + +void StartServicesSkill::send_start( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesSkill::send_start"); + std::cout << "StartServicesSkill::send_start"; + m_stateMachine.submitEvent("CMD_START"); + response->is_ok = true; +} + +void StartServicesSkill::send_stop( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + + RCLCPP_DEBUG(m_node->get_logger(), "StartServicesSkill::send_stop"); + std::cout << "StartServicesSkill::send_stop"; + m_stateMachine.submitEvent("CMD_STOP", QStateMachine::HighPriority); + response->is_ok = true; +} diff --git a/ros2prova/start_services_skill/src/StartServicesSkillSM.scxml b/ros2prova/start_services_skill/src/StartServicesSkillSM.scxml new file mode 100644 index 0000000..456bf0d --- /dev/null +++ b/ros2prova/start_services_skill/src/StartServicesSkillSM.scxml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + diff --git a/ros2prova/start_services_skill/src/main.cpp b/ros2prova/start_services_skill/src/main.cpp new file mode 100644 index 0000000..6b30594 --- /dev/null +++ b/ros2prova/start_services_skill/src/main.cpp @@ -0,0 +1,25 @@ +#include +#include +#include + + +#include +#include "StartServicesSkill.h" + +#include +#include + + + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + StartServicesSkill stateMachine("StartServicesSkill"); + stateMachine.start(argc, argv); + + int ret=app.exec(); + + return ret; + +} + diff --git a/ros2prova/stop_services_skill/CMakeLists.txt b/ros2prova/stop_services_skill/CMakeLists.txt new file mode 100644 index 0000000..cac145b --- /dev/null +++ b/ros2prova/stop_services_skill/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(stop_services_skill) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(lifecycle_msgs REQUIRED) +find_package(bt_interfaces REQUIRED) +find_package(other_interfaces REQUIRED) +find_package(Qt6 COMPONENTS Core Scxml StateMachine REQUIRED) +add_executable(${PROJECT_NAME} ) + +if (NOT Qt6_FOUND) + message("qt6 not found") +endif() + +# find dependencies +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +ament_target_dependencies(${PROJECT_NAME} bt_interfaces other_interfaces rclcpp rclcpp_lifecycle lifecycle_msgs) +target_link_libraries(${PROJECT_NAME} Qt6::Core Qt6::Scxml Qt6::StateMachine) +target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $) +target_sources( ${PROJECT_NAME} PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src/main.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/StopServicesSkill.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/StopServicesSkill.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/StopServicesDataModel.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/include/StopServicesDataModel.h) + + +install(TARGETS ${PROJECT_NAME} +DESTINATION lib/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +qt6_add_statecharts(${PROJECT_NAME} ${CMAKE_CURRENT_SOURCE_DIR}/src/StopServicesSkillSM.scxml) + +ament_package() diff --git a/ros2prova/stop_services_skill/include/StopServicesDataModel.h b/ros2prova/stop_services_skill/include/StopServicesDataModel.h new file mode 100644 index 0000000..2cf47f9 --- /dev/null +++ b/ros2prova/stop_services_skill/include/StopServicesDataModel.h @@ -0,0 +1,43 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/srv/get_state.hpp" +#include "lifecycle_msgs/srv/change_state.hpp" +#include + +using namespace std::chrono_literals; + +class StopServicesDataModel: public QScxmlCppDataModel +{ + Q_SCXML_DATAMODEL + +public: + StopServicesDataModel() = default; + bool stop(); + void set_name(std::string name); + bool setup(const QVariantMap& initialDataValues) override; +private: + std::shared_ptr m_node; + rclcpp::Client::SharedPtr m_client_get_state; + rclcpp::Client::SharedPtr m_client_change_state; + std::string m_name = "StopServicesDataModel"; + std::chrono::seconds m_timeout = 3s; + + bool is_client_available(const std::chrono::seconds& timeout = 3s); + std::optional get_state(const std::chrono::seconds& timeout = 3s); +}; + +Q_DECLARE_METATYPE(::StopServicesDataModel*) diff --git a/ros2prova/stop_services_skill/include/StopServicesSkill.h b/ros2prova/stop_services_skill/include/StopServicesSkill.h new file mode 100644 index 0000000..153c22d --- /dev/null +++ b/ros2prova/stop_services_skill/include/StopServicesSkill.h @@ -0,0 +1,42 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +# pragma once + +#include +#include +#include "StopServicesSkillSM.h" +#include "StopServicesDataModel.h" +#include +#include +#include +#include + +class StopServicesSkill +{ +public: + StopServicesSkill(std::string name ); + + bool start(int argc, char * argv[]); + static void spin(std::shared_ptr node); + void request_ack( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response); + void send_start( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + void send_stop([[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response); + +private: + std::shared_ptr m_threadSpin; + std::shared_ptr m_node; + rclcpp::Service::SharedPtr m_requestAckService; + rclcpp::Service::SharedPtr m_sendStartService; + rclcpp::Service::SharedPtr m_sendStopService; + std::string m_name; + StopServicesDataModel m_dataModel; + StopServicesSkillSM m_stateMachine; +}; diff --git a/ros2prova/stop_services_skill/package.xml b/ros2prova/stop_services_skill/package.xml new file mode 100644 index 0000000..71a46e5 --- /dev/null +++ b/ros2prova/stop_services_skill/package.xml @@ -0,0 +1,27 @@ + + + + stop_services_skill + 0.0.0 + A BT skill that stops the provided service + Stefano Bernagozzi + Apache-2.0 + + ament_cmake + bt_interfaces + other_interfaces + rclcpp + lifecycle_msgs + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + rosidl_default_runtime + + rosidl_default_generators + + + ament_cmake + + diff --git a/ros2prova/stop_services_skill/src/StopServicesDataModel.cpp b/ros2prova/stop_services_skill/src/StopServicesDataModel.cpp new file mode 100644 index 0000000..dcfec3e --- /dev/null +++ b/ros2prova/stop_services_skill/src/StopServicesDataModel.cpp @@ -0,0 +1,116 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ +#include "StopServicesDataModel.h" + +#include "lifecycle_msgs/msg/state.hpp" +#include "lifecycle_msgs/msg/transition.hpp" + +#include +#include +#include + + + +void StopServicesDataModel::set_name(std::string name) +{ + m_name=name; +} + +bool StopServicesDataModel::setup(const QVariantMap &initialDataValues) +{ + Q_UNUSED(initialDataValues) + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ 0, /*argv*/ nullptr); + } + + m_node = rclcpp::Node::make_shared(m_name); + m_client_get_state = m_node->create_client("/lc_talker/get_state"); + m_client_change_state = m_node->create_client("/lc_talker/change_state"); + + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesDataModel::start"); + std::cout << "StopServicesDataModel::start"; + + return true; +} + +bool StopServicesDataModel::is_client_available(const std::chrono::seconds& timeout) { + + // Check if client is available, else return + bool is_client_available = m_client_get_state->wait_for_service(timeout); + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Interrupted while waiting for the service. Exiting."); + return false; + } + if(!is_client_available){ + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "service not available, aborting."); + return false; + } + + return true; +} + +std::optional StopServicesDataModel::get_state(const std::chrono::seconds& timeout) { + + if(!is_client_available(timeout)) { + return std::nullopt; + } + + // Get the state of the managed node + auto request = std::make_shared(); + auto result = m_client_get_state->async_send_request(request); + + if (rclcpp::spin_until_future_complete(m_node, result,timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesDataModel::get_state(). Success"); + return result.get()->current_state; + } else { + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesDataModel::get_state(). Failure"); + return std::nullopt; + } + +} + +bool StopServicesDataModel::stop() { + + if(!is_client_available()){ + return false; + } + + // Check if managed node is in state active and make it inactive + // Check https://github.com/ros2/rcl_interfaces/blob/humble/lifecycle_msgs/msg/State.msg for reference + std::optional current_state = this->get_state(); + if(!current_state) { + return false; + } + + if(current_state.value().id != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_ERROR(m_node->get_logger(), "Cannot stop service since it is not active"); + return false; + } + else { + + // Change the state of the managed node to inactive + // Check https://github.com/ros2/rcl_interfaces/blob/humble/lifecycle_msgs/msg/Transition.msg for reference + auto request = std::make_shared(); + request->transition.id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE; + + auto result = m_client_change_state->async_send_request(request); + + if (rclcpp::spin_until_future_complete(m_node,result,m_timeout) == rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesDataModel::stop(). Success"); + return true; + } else { + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesDataModel::stop(). Failure"); + return false; + } + } + + return true; +} \ No newline at end of file diff --git a/ros2prova/stop_services_skill/src/StopServicesSkill.cpp b/ros2prova/stop_services_skill/src/StopServicesSkill.cpp new file mode 100644 index 0000000..58bfcd7 --- /dev/null +++ b/ros2prova/stop_services_skill/src/StopServicesSkill.cpp @@ -0,0 +1,98 @@ +/****************************************************************************** + * * + * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT) * + * All Rights Reserved. * + * * + ******************************************************************************/ + +#include "StopServicesSkill.h" + +#include +#include +#include +#include +#include + +StopServicesSkill::StopServicesSkill(std::string name ) : + m_name(std::move(name)) +{ + m_dataModel.set_name(name+"dataModel"); + m_stateMachine.setDataModel(&m_dataModel); +} + + +void StopServicesSkill::spin(std::shared_ptr node) +{ + rclcpp::spin(node); + rclcpp::shutdown(); +} + + +bool StopServicesSkill::start(int argc, char*argv[]) +{ + + if(!rclcpp::ok()) + { + rclcpp::init(/*argc*/ argc, /*argv*/ argv); + } + m_node = rclcpp::Node::make_shared(m_name); + + + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesSkill::start"); + std::cout << "StopServicesSkill::start"; + m_requestAckService = m_node->create_service("/StopServicesSkill/RequestAck", + std::bind(&StopServicesSkill::request_ack, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_sendStartService = m_node->create_service("/StopServicesSkill/SendStart", + std::bind(&StopServicesSkill::send_start, + this, + std::placeholders::_1, + std::placeholders::_2)); + m_sendStopService = m_node->create_service("/StopServicesSkill/SendStop", + std::bind(&StopServicesSkill::send_stop, + this, + std::placeholders::_1, + std::placeholders::_2)); + + m_stateMachine.start(); + m_threadSpin = std::make_shared(spin, m_node); + return true; +} + +void StopServicesSkill::request_ack( [[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response) +{ + auto message = bt_interfaces::msg::RequestAck(); + for (const auto& state : m_stateMachine.activeStateNames()) { + if (state == "stop") { + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_RUNNING; + } + if (state == "idle") { + m_stateMachine.submitEvent("CMD_OK"); + response->status.status = message.SKILL_IDLE; + } + } + response->is_ok = true; +} + +void StopServicesSkill::send_start( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesSkill::send_start"); + std::cout << "StopServicesSkill::send_start"; + m_stateMachine.submitEvent("CMD_START"); + response->is_ok = true; +} + +void StopServicesSkill::send_stop( [[maybe_unused]] const std::shared_ptr request, + [[maybe_unused]] std::shared_ptr response) +{ + + RCLCPP_DEBUG(m_node->get_logger(), "StopServicesSkill::send_stop"); + std::cout << "StopServicesSkill::send_stop"; + m_stateMachine.submitEvent("CMD_STOP", QStateMachine::HighPriority); + response->is_ok = true; +} diff --git a/ros2prova/stop_services_skill/src/StopServicesSkillSM.scxml b/ros2prova/stop_services_skill/src/StopServicesSkillSM.scxml new file mode 100644 index 0000000..8ad21c3 --- /dev/null +++ b/ros2prova/stop_services_skill/src/StopServicesSkillSM.scxml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + diff --git a/ros2prova/stop_services_skill/src/main.cpp b/ros2prova/stop_services_skill/src/main.cpp new file mode 100644 index 0000000..f22c94c --- /dev/null +++ b/ros2prova/stop_services_skill/src/main.cpp @@ -0,0 +1,25 @@ +#include +#include +#include + + +#include +#include "StopServicesSkill.h" + +#include +#include + + + +int main(int argc, char *argv[]) +{ + QCoreApplication app(argc, argv); + StopServicesSkill stateMachine("StopServicesSkill"); + stateMachine.start(argc, argv); + + int ret=app.exec(); + + return ret; + +} + From f88577676d5125b26bb8427b47fedc7c1f76721c Mon Sep 17 00:00:00 2001 From: fbrand-new Date: Fri, 9 Feb 2024 18:26:50 +0000 Subject: [PATCH 3/4] Check network using ping utility --- README.md | 4 +- .../include/CheckNetworkDataModel.h | 1 + .../src/CheckNetworkDataModel.cpp | 178 +++++++++++++----- 3 files changed, 134 insertions(+), 49 deletions(-) diff --git a/README.md b/README.md index 4c54675..2ca4b2a 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # Convince_simple_bts ## TODO -- Test connection lost between two machines. -- Make it sudoless. +- Test with unreachable site. Something that triggers the condition. - Modularly define the program to start and stop at runtime. [Right now it is a single program. Make it more than one] + what did i even mean here? - Turn Misael's code into a lifecycle node. \ No newline at end of file diff --git a/ros2prova/check_network_skill/include/CheckNetworkDataModel.h b/ros2prova/check_network_skill/include/CheckNetworkDataModel.h index a7bfa91..e7518f3 100644 --- a/ros2prova/check_network_skill/include/CheckNetworkDataModel.h +++ b/ros2prova/check_network_skill/include/CheckNetworkDataModel.h @@ -56,6 +56,7 @@ class CheckNetworkDataModel: public QScxmlCppDataModel //host can be either the name of the resource (e.g. r1-base) or its ip addr. bool isNetworkConnected(const std::string& host); + std::string exec(const char* cmd); // Calculating the Check Sum // TODO: understand what is this diff --git a/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp b/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp index 59dfa01..4d436cc 100644 --- a/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp +++ b/ros2prova/check_network_skill/src/CheckNetworkDataModel.cpp @@ -14,6 +14,7 @@ // Includes to perform ping #include +#include #include using namespace std::chrono_literals; @@ -34,7 +35,7 @@ bool CheckNetworkDataModel::setup(const QVariantMap &initialDataValues) } m_node = rclcpp::Node::make_shared(m_name); - m_address_name = "127.0.0.1"; + m_address_name = "example.com"; timer_ = m_node->create_wall_timer(1000ms, std::bind(&CheckNetworkDataModel::topic_callback, this)); @@ -69,68 +70,151 @@ void CheckNetworkDataModel::topic_callback() { std::cout << "Our machine is connected? " << m_is_connected; } + + bool CheckNetworkDataModel::isNetworkConnected(const std::string& host) { - struct addrinfo hints, *res; - struct sockaddr_in r_addr; - socklen_t r_addr_len; - int sockfd; + double threshold = 3000.; bool is_connected = false; + const std::string ping_command = "ping -c 4 -w 4 -q " + host + " 2>&1"; // Ping 4 times + std::string ping_output = exec(ping_command.c_str()); - // Setup hints - memset(&hints, 0, sizeof(hints)); - hints.ai_family = AF_INET; - hints.ai_socktype = SOCK_RAW; - hints.ai_protocol = IPPROTO_ICMP; + std::cout << "ping_output\n" << ping_output << std::endl; - // Get address info -> this will take care of the dns lookup if needed - if (getaddrinfo(host.c_str(), NULL, &hints, &res) != 0) { - return false; + if(ping_output.find("Temporary failure in name resolution") != std::string::npos){ + std::cout << "I am not connected to internet!" << std::endl; + is_connected = false; + return is_connected; } - // Create socket - // sockfd = socket(res->ai_family, res->ai_socktype, res->ai_protocol); - sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP); - if (sockfd == -1) { - std::cout << "Failed to create socket" << std::endl; - freeaddrinfo(res); - return false; + auto ping_output_stream = std::stringstream{ping_output}; + std::string line; + + std::string packets_summary_line; + std::string rtt_summary_line; + + std::vector ping_output_lines; + while(std::getline(ping_output_stream,line,'\n')) + { + ping_output_lines.push_back(line); } - // Build a packet to send to host - struct ping_pkt pckt; - memset(&pckt,0,sizeof(pckt)); - pckt.hdr.type = ICMP_ECHO; - pckt.hdr.un.echo.id = getpid(); - for (std::size_t i = 0; iai_addr,res->ai_addrlen); - if(bytes_sent == -1) { - std::cout << "Failed to send packet address" << std::endl; - freeaddrinfo(res); - return false; + if(ping_output_lines.size() >= 3){ + packets_summary_line = ping_output_lines[3]; + + } + if(ping_output_lines.size() >= 4){ + rtt_summary_line = ping_output_lines[4]; } - // Receive a packet - char rbuffer[128]; - int bytes_received = recvfrom(sockfd,rbuffer,sizeof(rbuffer),0, - (struct sockaddr*) &r_addr, &r_addr_len); - if(bytes_received == -1) { - std::cout << "Failed to receive packets" << std::endl; - is_connected = false; + std::cout << packets_summary_line << std::endl; + std::cout << rtt_summary_line << std::endl; + + std::regex rgx_packets(" (\\d*)\\% packet loss"); + std::regex rgx_ttl("= \\d*\\.\\d*\\/(\\d*[.]\\d*)\\/"); + std::smatch match_packets; + std::smatch match_ttl; + + if(std::regex_search(packets_summary_line, match_packets, rgx_packets)) + std::cout << "match:" << match_packets[1] << std::endl; + + if(std::regex_search(rtt_summary_line,match_ttl,rgx_ttl)) + std::cout << "match: " << match_ttl[1] << std::endl; + + double packet_loss = stod(match_packets[1]); + double rtt = stod(match_ttl[1]); + + if(packet_loss < 100.){ + if(rtt < threshold) + is_connected = true; + else + is_connected = false; } if(is_connected){ std::cout << "is connected!" << std::endl; } - freeaddrinfo(res); - return is_connected; + } + +std::string CheckNetworkDataModel::exec(const char* cmd) { + + std::array buffer; + std::string result; //The ping output string + std::unique_ptr pipe(popen(cmd, "r"), pclose); + if (!pipe) { + throw std::runtime_error("popen() failed!"); + } + while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) { + result += buffer.data(); + } + return result; +} + +// bool CheckNetworkDataModel::isNetworkConnected(const std::string& host) { + +// struct addrinfo hints, *res; +// struct sockaddr_in r_addr; +// socklen_t r_addr_len; +// int sockfd; +// bool is_connected = false; + +// // Setup hints +// memset(&hints, 0, sizeof(hints)); +// hints.ai_family = AF_INET; +// hints.ai_socktype = SOCK_RAW; +// hints.ai_protocol = IPPROTO_ICMP; + +// // Get address info -> this will take care of the dns lookup if needed +// if (getaddrinfo(host.c_str(), NULL, &hints, &res) != 0) { +// return false; +// } + +// // Create socket +// // sockfd = socket(res->ai_family, res->ai_socktype, res->ai_protocol); +// sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP); +// if (sockfd == -1) { +// std::cout << "Failed to create socket" << std::endl; +// freeaddrinfo(res); +// return false; +// } + +// // Build a packet to send to host +// struct ping_pkt pckt; +// memset(&pckt,0,sizeof(pckt)); +// pckt.hdr.type = ICMP_ECHO; +// pckt.hdr.un.echo.id = getpid(); +// for (std::size_t i = 0; iai_addr,res->ai_addrlen); +// if(bytes_sent == -1) { +// std::cout << "Failed to send packet address" << std::endl; +// freeaddrinfo(res); +// return false; +// } + +// // Receive a packet +// char rbuffer[128]; +// int bytes_received = recvfrom(sockfd,rbuffer,sizeof(rbuffer),0, +// (struct sockaddr*) &r_addr, &r_addr_len); +// if(bytes_received == -1) { +// std::cout << "Failed to receive packets" << std::endl; +// is_connected = false; +// } + +// if(is_connected){ +// std::cout << "is connected!" << std::endl; +// } + +// freeaddrinfo(res); + +// return is_connected; +// } From e039035b16e33839be05d40b30e23d6f7b90651e Mon Sep 17 00:00:00 2001 From: fbrand-new Date: Sat, 10 Feb 2024 13:26:22 +0000 Subject: [PATCH 4/4] Changed node to be started --- ros2prova/start_services_skill/src/StartServicesDataModel.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2prova/start_services_skill/src/StartServicesDataModel.cpp b/ros2prova/start_services_skill/src/StartServicesDataModel.cpp index 188e5c5..d9df905 100644 --- a/ros2prova/start_services_skill/src/StartServicesDataModel.cpp +++ b/ros2prova/start_services_skill/src/StartServicesDataModel.cpp @@ -30,8 +30,8 @@ bool StartServicesDataModel::setup(const QVariantMap &initialDataValues) } m_node = rclcpp::Node::make_shared(m_name); - m_client_get_state = m_node->create_client("/lc_talker/get_state"); - m_client_change_state = m_node->create_client("/lc_talker/change_state"); + m_client_get_state = m_node->create_client("/dr_spaam_ros_node/get_state"); + m_client_change_state = m_node->create_client("/dr_spaam_ros_node/change_state"); RCLCPP_DEBUG(m_node->get_logger(), "StartServicesDataModel::start"); std::cout << "StartServicesDataModel::start";