[tests] Restructure test_sot_loader.

Author: olivier-stasse

tests/CMakeLists.txt

@@ -115,6 +115,10 @@ if(BUILD_TESTING)
     ament_target_dependencies(${current_test_name} dynamic_graph_bridge_msgs
                               rclcpp rcl_interfaces std_srvs)
 
+    # Install tests
+    install(TARGETS test_${test_name}
+      DESTINATION lib/${PROJECT_NAME})
+
   endmacro(create_ros_bridge_unittest test_name)
 
   # C++ unit-tests.

tests/impl_test_sot_external_interface.cpp

@@ -25,6 +25,7 @@ ImplTestSotExternalInterface::ImplTestSotExternalInterface(
 ImplTestSotExternalInterface::~ImplTestSotExternalInterface() {}
 
 void ImplTestSotExternalInterface::init() {
+  std::cout << "ImplTestSotExternalInterface::init - begin" << std::endl;
   std::vector<double> ctrl_vector;
   ctrl_vector.resize(2);
 
@@ -52,11 +53,23 @@ void ImplTestSotExternalInterface::init() {
 
   RosNodePtr py_inter_ptr = get_ros_node("python_interpreter");
   // Set the control time step parameter to 0.001
-  double ts = 0.001;
+  double ts = 0.01;
 
+  // Publish parameters related to the control interface
   py_inter_ptr->declare_parameter<double>("/sot_controller/dt", ts);
+  // Create services to interact with the embedded python interpreter.
   py_interpreter_srv_->start_ros_service();
+
+  // Non blocking spinner to deal with ros.
+  // Be careful: here with tests we do not care about real time issue.
+  // In the real robot you need to make sure that the control loop is
+  // not block and that the thread associated with the services is not blocking
+  // the control loop.
+  ros_spin_non_blocking();
+
   device_->timeStep(ts);
+
+  std::cout << "ImplTestSotExternalInterface::init - end" << std::endl;
 }
 
 void ImplTestSotExternalInterface::setupSetSensors(
@@ -66,7 +79,7 @@ void ImplTestSotExternalInterface::setupSetSensors(
 
 void ImplTestSotExternalInterface::nominalSetSensors(
     std::map<std::string, dynamicgraph::sot::SensorValues> &) {
-  std::cout << "ImplTestSotExternalInterface::nominalSetSensors" << std::endl;
+  //  std::cout << "ImplTestSotExternalInterface::nominalSetSensors" << std::endl;
 }
 
 void ImplTestSotExternalInterface::cleanupSetSensors(

tests/test_sot_loader.cpp

@@ -3,10 +3,14 @@
 #include <gmock/gmock.h>
 #pragma clang diagnostic pop
 
+using namespace std::chrono_literals;
+
+#include <rclcpp/rclcpp.hpp>
+
 #include "dynamic_graph_bridge/ros.hpp"
 #include "dynamic_graph_bridge/sot_loader.hh"
 #include "dynamic_graph_bridge_msgs/msg/vector.hpp"
-#include "test_common.hpp"
+#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
 
 namespace test_sot_loader {
 int l_argc;
@@ -21,29 +25,131 @@ class MockSotLoaderTest : public ::testing::Test {
    public:
     rclcpp::Subscription<dynamic_graph_bridge_msgs::msg::Vector>::SharedPtr
     subscription_;
+    bool service_done_;
+    std::string node_name_;
+    std::string response_dg_python_result_;
 
-    ~MockSotLoader() {}
+    MockSotLoader():
+        node_name_("unittest_sot_loader") {}
 
-    void topic_callback(const dynamic_graph_bridge_msgs::msg::Vector::SharedPtr msg) const {
-      auto lsize=msg->data.size();
+    ~MockSotLoader() {
+      service_done_ = false;
+      response_dg_python_result_ = "";
     }
 
+    void topic_callback(const dynamic_graph_bridge_msgs::msg::Vector::SharedPtr msg) const {
+      bool ok=true;
+      for(std::vector<double>::size_type idx=0;idx<msg->data.size();idx++) {
+        if (msg->data[idx]!=0.0)
+          ok=false;
+      }
+      ASSERT_TRUE(ok);
+    }
+    // This method is to listen to publication from the control signal (dg world)
+    // to the control_ros topic (ros world).
     void subscribe_to_a_topic() {
-      subscription_ = create_subscription<dynamic_graph_bridge_msgs::msg::Vector>(
-          "control_ros", 1, std::bind(&MockSotLoader::topic_callback, this,
-                                      std::placeholders::_1));
+      subscription_ = dynamic_graph_bridge::get_ros_node(node_name_)->
+          create_subscription<dynamic_graph_bridge_msgs::msg::Vector>(
+              "control_ros", 1, std::bind(&MockSotLoader::topic_callback, this,
+                                          std::placeholders::_1));
+    }
+
+    // This method is to receive the result of client request to run_python_file
+    //
+    void response_dg_python(
+        const rclcpp::Client<dynamic_graph_bridge_msgs::srv::RunPythonFile>::SharedFuture
+        future) {
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"), "response_dg_python:");
+      auto status = future.wait_for(500ms);
+      if (status == std::future_status::ready) {
+        // uncomment below line if using Empty() message
+        // RCLCPP_INFO(this->get_logger(), "Result: success");
+        // comment below line if using Empty() message
+        RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                    "response_dg_python - Result: %s",
+                    future.get()->result.c_str());
+        service_done_ = true;
+        response_dg_python_result_ = future.get()->result;
+      } else {
+        RCLCPP_INFO(rclcpp::get_logger("dynmic_graph_bridge"),
+                    "response_dg_python - Service In-Progress...");
+      }
+    }
+
+    void start_run_python_script_ros_service(const std::string& file_name) {
+      // Service name.
+      std::string service_name = "/dynamic_graph_bridge/run_python_file";
+      // Create a client from a temporary node.
+      auto client = dynamic_graph_bridge::get_ros_node(node_name_)->
+          create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+              service_name);
+      ASSERT_TRUE(client->wait_for_service(1s));
+
+      // Fill the command message.
+      dynamic_graph_bridge_msgs::srv::RunPythonFile::Request::SharedPtr request =
+          std::make_shared<
+            dynamic_graph_bridge_msgs::srv::RunPythonFile::Request>();
+      request->input = file_name;
+      // Call the service.
+      auto response =
+          client->async_send_request(request,
+                                     std::bind(&MockSotLoader::response_dg_python,
+                                               this,
+                                               std::placeholders::_1));
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Send request to service %s - Waiting",
+                  service_name.c_str());
+      response.wait();
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Get the answer");
+
+    }
+
+    void display_services(
+        std::map<std::string, std::vector<std::string> >& list_service_and_types) {
+      for (std::map<std::string, std::vector<std::string> >::const_iterator
+               const_it = list_service_and_types.begin();
+           const_it != list_service_and_types.end(); ++const_it) {
+        std::cerr << const_it->first << "\t\t\t";
+        for (unsigned i = 0; i < const_it->second.size(); ++i) {
+          std::cerr << std::right;
+          std::cerr << const_it->second[i] << std::endl;
+        }
+      }
+    }
+
+    void local_ros_spin() {
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "local_ros_spin");
+      dynamic_graph_bridge::ros_spin();
     }
 
     void generateEvents() {
       usleep(20000);
-      std::cerr << "Start Dynamic Graph " << std::endl;
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Start Dynamic Graph ");
       dynamic_graph_stopped_ = false;
 
-      usleep(20000);
-      std::cerr << "Stop Dynamic Graph " << std::endl;
+      std::string file_name =
+          TEST_CONFIG_PATH + std::string("simple_ros_publish.py");
+      std::string result = "";
+      std::string standard_output = "";
+      std::string standard_error = "";
+      start_run_python_script_ros_service(file_name);
+
+
+      usleep(100000);
+      std::map<std::string, std::vector<std::string>> list_service_and_types;
+      dynamic_graph_bridge::RosNodePtr ros_node = dynamic_graph_bridge::get_ros_node(node_name_);
+      bool simple_service_exists = false;
+
+      usleep(30720000); // Wait 30s
+      RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+                  "Stop Dynamic Graph ");
       dynamic_graph_stopped_ = true;
     }
 
+
     void testLoadController() {
       // Set input  call
       int argc = 2;
@@ -79,16 +185,11 @@ class MockSotLoaderTest : public ::testing::Test {
       // Start the thread generating events.
       std::thread local_events(&MockSotLoader::generateEvents, this);
 
-      // Create and call the clients.
-      std::string file_name =
-          TEST_CONFIG_PATH + std::string("simple_ros_publish.py");
-      std::string result = "";
-      std::string standard_output = "";
-      std::string standard_error = "";
-      //start_run_python_script_ros_service(file_name, result);
+      // Start the thread for ros events.
+      std::thread local_ros_spin_thread(&MockSotLoader::local_ros_spin, this);
 
       // Wait for each threads.
-      SotLoader::lthread_join();  // Wait 100 ms
+      SotLoader::lthread_join();
       local_events.join();
     }
   };
@@ -111,11 +212,13 @@ class MockSotLoaderTest : public ::testing::Test {
   void SetUp() {
     mockSotLoader_ptr_ = new MockSotLoader();
     mockSotLoader_ptr_->initialize();
+    dynamic_graph_bridge::ros_spin_non_blocking();
   }
 
   void TearDown() {
     delete mockSotLoader_ptr_;
     mockSotLoader_ptr_ = nullptr;
+    dynamic_graph_bridge::ros_clean();
   }
 };