Add check network skill, start and stop skill

.gitignore

@@ -6,4 +6,11 @@
 /compile_commands.json
 /.clangd/
 CMakeLists.txt.user*
-/ros2_example/
+/ros2_example/
+.vscode
+.devcontainer
+ros2prova/**/build
+ros2prova/**/install
+ros2prova/**/log
+ros2prova/lifecycle_py/*
+ros2prova/stop_start_component

README.md

@@ -1 +1,7 @@
-# Convince_simple_bts
+# Convince_simple_bts
+
+## TODO
+- 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.

ros2prova/BT/simple_bt.xml

@@ -3,11 +3,14 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <ReactiveSequence>
-            <ReactiveFallback>
+            <!-- <ReactiveFallback>
                 <Condition ID="ROS2Condition" interface="service" name="BatteryLevel" nodeName="BatteryLevelSkill" topicName="/BatteryLevelSkill"/>
                 <Action ID="ROS2Action" interface="service" name="Alarm" nodeName="AlarmBatteryLowSkill" topicName="/AlarmBatteryLowSkill"/>
-            </ReactiveFallback>
-            <Action ID="ROS2Action" interface="service" name="BatteryDrainer" nodeName="BatteryDrainerSkill" topicName="/BatteryDrainerSkill"/>
+            </ReactiveFallback> -->
+            <!-- <Action ID="ROS2Action" interface="service" name="BatteryDrainer" nodeName="BatteryDrainerSkill" topicName="/BatteryDrainerSkill"/> -->
+            <Action ID="ROS2Action" interface="service" name="CheckNetwork" nodeName="CheckNetworkSkill" topicName="/CheckNetworkSkill" />
+            <!-- <Action ID="ROS2Action" interface="service" name="StartServices" nodeName="StartServicesSkill" topicName="/StartServicesSkill" />
+            <Action ID="ROS2Action" interface="service" name="StopServices" nodeName="StopServicesSkill" topicName="/StopServicesSkill" /> -->
         </ReactiveSequence>
     </BehaviorTree>
     <!-- ////////// -->

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(<dependency> 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
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>)
+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()

ros2prova/check_network_skill/include/CheckNetworkDataModel.h

@@ -0,0 +1,82 @@
+/******************************************************************************
+ *                                                                            *
+ * Copyright (C) 2020 Fondazione Istituto Italiano di Tecnologia (IIT)        *
+ * All Rights Reserved.                                                       *
+ *                                                                            *
+ ******************************************************************************/
+
+#ifndef __CHECKNETWORKDATAMODEL_H_
+#define __CHECKNETWORKDATAMODEL_H_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <QScxmlCppDataModel>
+#include <QVariantMap>
+#include <QTime>
+#include <QTimer>
+#include <QDebug>
+
+// Ping utility structs
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netdb.h>
+#include <unistd.h>
+#include <arpa/inet.h>
+#include <netinet/ip_icmp.h>
+
+// 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<std::thread> m_thread;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    std::string m_address_name;
+    std::shared_ptr<rclcpp::Node> 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);
+    std::string exec(const char* cmd);
+
+    // Calculating the Check Sum
+    // TODO: understand what is this
+    unsigned short checksum(void* b, int len)
+    {
+        unsigned short* buf = static_cast<unsigned short*>(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

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 <thread>
+#include <rclcpp/rclcpp.hpp>
+#include "CheckNetworkSkillSM.h"
+#include "CheckNetworkDataModel.h"
+#include <bt_interfaces/msg/request_ack.hpp>
+#include <bt_interfaces/srv/request_ack.hpp>
+#include <bt_interfaces/srv/send_start.hpp>
+#include <bt_interfaces/srv/send_stop.hpp>
+
+class CheckNetworkSkill
+{
+public:
+    CheckNetworkSkill(std::string name );
+
+    bool start(int argc, char * argv[]);
+    static void spin(std::shared_ptr<rclcpp::Node> node);
+    void request_ack( [[maybe_unused]] const std::shared_ptr<bt_interfaces::srv::RequestAck::Request> request,
+          std::shared_ptr<bt_interfaces::srv::RequestAck::Response>      response);
+    void send_start( [[maybe_unused]] const std::shared_ptr<bt_interfaces::srv::SendStart::Request> request,
+                     [[maybe_unused]] std::shared_ptr<bt_interfaces::srv::SendStart::Response>      response);
+    void send_stop([[maybe_unused]] const std::shared_ptr<bt_interfaces::srv::SendStop::Request> request,
+                   [[maybe_unused]] std::shared_ptr<bt_interfaces::srv::SendStop::Response>      response);
+
+private:
+    std::shared_ptr<std::thread> m_threadSpin;
+    std::shared_ptr<rclcpp::Node> m_node;
+    rclcpp::Service<bt_interfaces::srv::RequestAck>::SharedPtr m_requestAckService;
+    rclcpp::Service<bt_interfaces::srv::SendStart>::SharedPtr m_sendStartService;
+    rclcpp::Service<bt_interfaces::srv::SendStop>::SharedPtr m_sendStopService;
+    std::string m_name;
+    CheckNetworkDataModel m_dataModel;
+    CheckNetworkSkillSM m_stateMachine;
+};

ros2prova/check_network_skill/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>check_network_skill</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">Stefano Bernagozzi</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>bt_interfaces</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>