diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index c4e0d47b92ad7619f3cc554407bee9e403b14ad9..184907bec4aafe1087ce682b669567413fee992a 100644
--- a/tests/CMakeLists.txt
+++ b/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.
diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index 607ca5e837132443d198320019f7c6e0246e3f2c..34b010064688502734f1129ec1e101c9d881fb20 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/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(
diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index dcaff408d596b1711d1c7309762f90d1ee1df678..12b32e159688a3849ad557ed6baf99644e972a2a 100644
--- a/tests/test_sot_loader.cpp
+++ b/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();
}
};