From 9ad64efdb02c3724d79fb1fcc53b47d37abfa980 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Fri, 24 Nov 2023 14:22:18 +0100
Subject: [PATCH] Making pre-commit happy.
---
.../dynamic_graph_bridge/ros_parameter.hpp | 10 --
.../dynamic_graph_bridge/sot_loader_basic.hh | 5 +-
src/CMakeLists.txt | 16 +--
src/ros_parameter.cpp | 53 -------
src/sot_loader_basic.cpp | 92 +++++++++----
tests/CMakeLists.txt | 30 ++--
tests/test_sot_loader_basic.cpp | 130 +++---------------
7 files changed, 117 insertions(+), 219 deletions(-)
delete mode 100644 include/dynamic_graph_bridge/ros_parameter.hpp
delete mode 100644 src/ros_parameter.cpp
diff --git a/include/dynamic_graph_bridge/ros_parameter.hpp b/include/dynamic_graph_bridge/ros_parameter.hpp
deleted file mode 100644
index fda58a3..0000000
--- a/include/dynamic_graph_bridge/ros_parameter.hpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#ifndef _ROS_DYNAMIC_GRAPH_PARAMETER_
-#define _ROS_DYNAMIC_GRAPH_PARAMETER_
-
-#include "rclcpp/node.hpp"
-namespace dynamicgraph {
-
-bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh);
-
-}
-#endif /* _ROS_DYNAMIC_GRAPH_PARAMETER_ */
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index 8390448..35ac763 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -84,7 +84,7 @@ class SotLoaderBasic : public rclcpp::Node {
std::vector<std::string> stateVectorMap_;
public:
- SotLoaderBasic(const std::string & aNodeName=std::string("SotLoaderBasic"));
+ SotLoaderBasic(const std::string& aNodeName = std::string("SotLoaderBasic"));
virtual ~SotLoaderBasic();
// \brief Read user input to extract the path of the SoT dynamic library.
@@ -124,6 +124,9 @@ class SotLoaderBasic : public rclcpp::Node {
// \brief Specify the name of the dynamic library.
void setDynamicLibraryName(std::string& afilename);
+
+ // \brief Read the parameters of the node
+ bool parameter_server_read_robot_description();
};
#endif /* _SOT_LOADER_BASIC_HH_ */
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 96e72da..c8df4cb 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -5,7 +5,6 @@
# Main Library
set(${PROJECT_NAME}_HEADERS
- ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_parameter.hpp
${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_python_interpreter_client.hpp
${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME}/ros_python_interpreter_server.hpp
${PROJECT_SOURCE_DIR}/src/dg_ros_mapping.hpp
@@ -16,7 +15,6 @@ set(${PROJECT_NAME}_SOURCES
${PROJECT_SOURCE_DIR}/src/ros_python_interpreter_client.cpp #
${PROJECT_SOURCE_DIR}/src/ros_python_interpreter_server.cpp #
${PROJECT_SOURCE_DIR}/src/ros.cpp #
- ${PROJECT_SOURCE_DIR}/src/ros_parameter.cpp #
)
add_library(ros_bridge SHARED ${${PROJECT_NAME}_SOURCES}
${${PROJECT_NAME}_HEADERS})
@@ -64,13 +62,13 @@ foreach(plugin ${plugins})
${PROJECT_VERSION})
endif(SUFFIX_SO_VERSION)
target_link_libraries(
- ${plugin_library_name} ${${plugin_library_name}_deps}
- ros_bridge
- dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
- rclcpp::rclcpp
- rcl_interfaces::rcl_interfaces__rosidl_typesupport_cpp
- std_srvs::std_srvs__rosidl_typesupport_cpp
- )
+ ${plugin_library_name}
+ ${${plugin_library_name}_deps}
+ ros_bridge
+ dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
+ rclcpp::rclcpp
+ rcl_interfaces::rcl_interfaces__rosidl_typesupport_cpp
+ std_srvs::std_srvs__rosidl_typesupport_cpp)
target_include_directories(
${plugin_library_name}
PUBLIC $<INSTALL_INTERFACE:include>
diff --git a/src/ros_parameter.cpp b/src/ros_parameter.cpp
deleted file mode 100644
index 7904289..0000000
--- a/src/ros_parameter.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-#include "dynamic_graph_bridge/ros_parameter.hpp"
-
-#include <urdf_parser/urdf_parser.h>
-
-#include <boost/make_shared.hpp>
-#include <boost/shared_ptr.hpp>
-#include <sot/core/robot-utils.hh>
-#include <stdexcept>
-
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/parsers/urdf.hpp"
-
-namespace dynamicgraph {
-bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh) {
- if (!nh->has_parameter("robot_description")) {
- nh->declare_parameter("robot_description", std::string(""));
- }
-
- std::string robot_description;
- std::string parameter_name("robot_description");
- nh->get_parameter(parameter_name, robot_description);
- if (robot_description.empty()) {
- RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
- "Parameter robot_description is empty");
- return false;
- }
-
- std::string model_name("robot");
-
- // Search for the robot util related to robot_name.
- sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
- // If does not exist then it is created.
- if (aRobotUtil == sot::RefVoidRobotUtil())
- aRobotUtil = sot::createRobotUtil(model_name);
-
- // If the creation is fine
- if (aRobotUtil != sot::RefVoidRobotUtil()) {
- // Then set the robot model.
- aRobotUtil->set_parameter(parameter_name, robot_description);
- RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
- "parameter_server_read_robot_description : Set parameter_name : %s.", parameter_name.c_str());
- // Everything went fine.
- return true;
- }
- RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
- "Wrong initialization of parameter_name %s",
- parameter_name.c_str());
-
- // Otherwise something went wrong.
- return false;
-}
-
-} // namespace dynamicgraph
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index 6a04bc5..d6023bd 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -13,9 +13,14 @@
#include <dynamic-graph/pool.h>
+#include <boost/make_shared.hpp>
+#include <boost/shared_ptr.hpp>
#include <exception>
+#include <sot/core/robot-utils.hh>
+#include <stdexcept>
-#include "dynamic_graph_bridge/ros_parameter.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
// POSIX.1-2001
#include <dlfcn.h>
@@ -24,15 +29,15 @@ using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
-SotLoaderBasic::SotLoaderBasic(const std::string &aNodeName)
+SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
: rclcpp::Node(aNodeName),
dynamic_graph_stopped_(true),
+ sotController_(NULL),
sotRobotControllerLibrary_(0) {
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
"Beginning of SotLoaderBasic");
- // nh_ = dynamic_graph_bridge::get_ros_node("SotLoaderBasic");
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
- "End of SotLoaderBasic");
+ "End of SotLoaderBasic");
}
SotLoaderBasic::~SotLoaderBasic() {
@@ -41,23 +46,23 @@ SotLoaderBasic::~SotLoaderBasic() {
void SotLoaderBasic::initialize() {}
-//rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() { return nh_; }
int SotLoaderBasic::initPublication() {
// Prepare message to be published
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
"SotLoaderBasic::initPublication - create joint_pub");
-
+
joint_pub_ =
create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
- "SotLoaderBasic::initPublication - after create joint_pub");
+ "SotLoaderBasic::initPublication - after create joint_pub");
return 0;
}
void SotLoaderBasic::initializeServices() {
- RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
- "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
+ RCLCPP_INFO(
+ rclcpp::get_logger("dynamic_graph_bridge"),
+ "SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
using namespace std::placeholders;
service_start_ = create_service<std_srvs::srv::Empty>(
@@ -74,8 +79,7 @@ void SotLoaderBasic::initializeServices() {
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
"SotLoaderBasic::initializeServices - stopped dynamic graph.");
- rclcpp::Node::SharedPtr a_node_ptr(this);
- dynamicgraph::parameter_server_read_robot_description(a_node_ptr);
+ parameter_server_read_robot_description();
}
void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
@@ -106,8 +110,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
RCLCPP_INFO(get_logger(), "state_vector_map parameter size %ld",
stateVectorMap_.size());
} catch (exception& e) {
- std::throw_with_nested(
- std::logic_error("Unable to call nh_->get_parameter"));
+ std::throw_with_nested(std::logic_error("Unable to call get_parameter"));
}
nbOfJoints_ = static_cast<int>(stateVectorMap_.size());
@@ -233,17 +236,19 @@ void SotLoaderBasic::CleanUp() {
// SignalCaster singleton could probably be destroyed.
// Load the symbols.
- destroySotExternalInterface_t* destroySot =
- reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
- dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
- const char* dlsym_error = dlerror();
- if (dlsym_error) {
- std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
- return;
- }
+ if (sotController_ != NULL) {
+ destroySotExternalInterface_t* destroySot =
+ reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
+ dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+ const char* dlsym_error = dlerror();
+ if (dlsym_error) {
+ std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
+ return;
+ }
- destroySot(sotController_);
- sotController_ = NULL;
+ destroySot(sotController_);
+ sotController_ = NULL;
+ }
/// Uncount the number of access to this library.
try {
@@ -265,3 +270,44 @@ void SotLoaderBasic::stop_dg(
std::shared_ptr<std_srvs::srv::Empty::Response>) {
dynamic_graph_stopped_ = true;
}
+
+bool SotLoaderBasic::parameter_server_read_robot_description() {
+ if (!has_parameter("robot_description")) {
+ declare_parameter("robot_description", std::string(""));
+ }
+
+ std::string robot_description;
+ std::string parameter_name("robot_description");
+ get_parameter(parameter_name, robot_description);
+ if (robot_description.empty()) {
+ RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Parameter robot_description is empty");
+ return false;
+ }
+
+ std::string model_name("robot");
+
+ // Search for the robot util related to robot_name.
+ RobotUtilShrPtr aRobotUtil = getRobotUtil(model_name);
+ // If does not exist then it is created.
+ if (aRobotUtil == RefVoidRobotUtil())
+ aRobotUtil = createRobotUtil(model_name);
+
+ // If the creation is fine
+ if (aRobotUtil != RefVoidRobotUtil()) {
+ // Then set the robot model.
+ aRobotUtil->set_parameter(parameter_name, robot_description);
+ RCLCPP_INFO(
+ rclcpp::get_logger("dynamic_graph_bridge"),
+ "parameter_server_read_robot_description : Set parameter_name : %s.",
+ parameter_name.c_str());
+ // Everything went fine.
+ return true;
+ }
+ RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Wrong initialization of parameter_name %s",
+ parameter_name.c_str());
+
+ // Otherwise something went wrong.
+ return false;
+}
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index b8472eb..2a7b4dd 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -15,33 +15,35 @@ if(BUILD_TESTING)
set_target_properties(impl_test_sot_mock_device PROPERTIES INSTALL_RPATH
$ORIGIN)
target_link_libraries(
- impl_test_sot_mock_device PUBLIC sot-core::sot-core
- dynamic-graph-python::dynamic-graph-python
- ros_bridge
- )
+ impl_test_sot_mock_device
+ PUBLIC sot-core::sot-core dynamic-graph-python::dynamic-graph-python
+ ros_bridge)
install(
TARGETS impl_test_sot_mock_device
EXPORT ${TARGETS_EXPORT_NAME}
DESTINATION ${DYNAMIC_GRAPH_PLUGINDIR})
- file(MAKE_DIRECTORY
- ${PROJECT_BINARY_DIR}/tests/dynamic_graph/ros/tests/impl_test_sot_mock_device)
-
+ file(
+ MAKE_DIRECTORY
+ ${PROJECT_BINARY_DIR}/tests/dynamic_graph/ros/tests/impl_test_sot_mock_device
+ )
+
dynamic_graph_python_module(
- "${PYTHON_DIR}/ros/tests/impl_test_sot_mock_device" impl_test_sot_mock_device
- dynamic_graph_bridge-impl_test_sot_mock_device-wrap SOURCE_PYTHON_MODULE
+ "${PYTHON_DIR}/ros/tests/impl_test_sot_mock_device"
+ impl_test_sot_mock_device
+ dynamic_graph_bridge-impl_test_sot_mock_device-wrap
+ SOURCE_PYTHON_MODULE
"${CMAKE_CURRENT_SOURCE_DIR}/impl_test_sot_mock_device-python-module-py.cc")
# Library for sot_external_interface
add_library(impl_test_library SHARED impl_test_sot_external_interface.cpp)
- set_target_properties(impl_test_library PROPERTIES INSTALL_RPATH
- $ORIGIN)
+ set_target_properties(impl_test_library PROPERTIES INSTALL_RPATH $ORIGIN)
target_link_libraries(impl_test_library PUBLIC impl_test_sot_mock_device
sot-core::sot-core)
ament_target_dependencies(impl_test_library PUBLIC dynamic_graph_bridge_msgs
- rclcpp rcl_interfaces std_srvs)
+ rclcpp rcl_interfaces std_srvs)
install(
TARGETS impl_test_library
@@ -99,7 +101,11 @@ if(BUILD_TESTING)
PRIVATE ${PROJECT_SOURCE_DIR}/include)
# Link the dependecies to it.
+ #
target_link_libraries(test_${test_name} ros_bridge)
+ set(current_test_name test_${test_name})
+ ament_target_dependencies(${current_test_name} dynamic_graph_bridge_msgs
+ rclcpp rcl_interfaces std_srvs)
# add some preprocessor variable
target_compile_definitions(
diff --git a/tests/test_sot_loader_basic.cpp b/tests/test_sot_loader_basic.cpp
index a052403..b60f249 100644
--- a/tests/test_sot_loader_basic.cpp
+++ b/tests/test_sot_loader_basic.cpp
@@ -6,23 +6,21 @@
#include "dynamic_graph_bridge/ros.hpp"
#include "dynamic_graph_bridge/sot_loader_basic.hh"
-
namespace test_sot_loader_basic {
int l_argc;
-char** l_argv;
-}
+char **l_argv;
+} // namespace test_sot_loader_basic
class MockSotLoaderBasicTest : public ::testing::Test {
public:
class MockSotLoaderBasic : public SotLoaderBasic {
public:
- MockSotLoaderBasic(const std::string &aNodeName=
- std::string("MockSotLoaderBasic")) :
- SotLoaderBasic(aNodeName)
- {}
+ MockSotLoaderBasic(
+ const std::string &aNodeName = std::string("MockSotLoaderBasic"))
+ : SotLoaderBasic(aNodeName) {}
virtual ~MockSotLoaderBasic() {}
- void checkStateVectorMap() {
+ void UniqueTest() {
// This test makes sure that the class test_sot_loader_basic is able to
// read parameters from the ROS-2 space.
// It is suppose to read :
@@ -35,9 +33,7 @@ class MockSotLoaderBasicTest : public ::testing::Test {
EXPECT_TRUE(aJoint1 == stateVectorMap_[0]);
std::string aJoint2("joint2");
EXPECT_TRUE(aJoint2 == stateVectorMap_[1]);
- }
- void testParseOptions() {
// Void call
int argc = 1;
char argv1[30] = "mocktest";
@@ -61,50 +57,13 @@ class MockSotLoaderBasicTest : public ::testing::Test {
// Check that the file is what we specified
std::string finalname("libimpl_test_library.so");
EXPECT_TRUE(finalname == dynamicLibraryName_);
- }
- void testInitializeRosNode() {
- int argc = 1;
- char *argv[1];
- char argv1[30] = "mocktest";
- argv[0] = argv1;
- parseOptions(argc, argv);
initPublication();
EXPECT_TRUE(joint_pub_ != 0);
initializeServices();
EXPECT_TRUE(service_start_ != 0);
EXPECT_TRUE(service_stop_ != 0);
- }
-
- void testJointStatesPublication() {
- int argc = 1;
- char *argv[1];
- char argv1[30] = "mocktest";
- argv[0] = argv1;
- parseOptions(argc, argv);
- }
- void testInitialization() {
- // Set input arguments
- int argc = 2;
- char *argv[2];
-
- char argv1[30] = "mocktest";
- argv[0] = argv1;
- char argv2[60] = "--input-file=libimpl_test_library.so";
- argv[1] = argv2;
- parseOptions(argc, argv);
-
- std::string finalname("libimpl_test_library.so");
- EXPECT_TRUE(finalname == dynamicLibraryName_);
-
- // Performs initializatio of libimpl_test_library.so
- loadController();
- EXPECT_TRUE(sotRobotControllerLibrary_ != 0);
- EXPECT_TRUE(sotController_ != nullptr);
- }
-
- void test_start_stop_dg() {
std::shared_ptr<std_srvs::srv::Empty::Request> empty_rqst;
std::shared_ptr<std_srvs::srv::Empty::Response> empty_resp;
start_dg(empty_rqst, empty_resp);
@@ -112,99 +71,48 @@ class MockSotLoaderBasicTest : public ::testing::Test {
stop_dg(empty_rqst, empty_resp);
EXPECT_TRUE(dynamic_graph_stopped_);
- }
-
- void test_cleanup() {
- CleanUp();
- EXPECT_TRUE(sotController_ == NULL);
- std::cout << "sotController_: " << sotController_ << std::endl;
- // Set input arguments
- int argc = 2;
- char *argv[2];
-
- char argv1[30] = "mocktest";
- argv[0] = argv1;
- char argv2[60] = "--input-file=libimpl_test_library.so";
- argv[1] = argv2;
- parseOptions(argc, argv);
-
- std::string finalname("libimpl_test_library.so");
- EXPECT_TRUE(finalname == dynamicLibraryName_);
-
- // Performs initializatio of libimpl_test_library.so
- loadController();
// Remove
CleanUp();
}
};
public:
- MockSotLoaderBasic *mockSotLoaderBasic_ptr_;
+ std::shared_ptr<MockSotLoaderBasic> mockSotLoaderBasic_ptr_;
unsigned int nb_tests_;
- MockSotLoaderBasicTest() {
- nb_tests_ = 0;
- }
-
+ MockSotLoaderBasicTest() { nb_tests_ = 0; }
+
// For the set of tests coded in this file.
static void SetUpTestCase() {
-
- rclcpp::init(test_sot_loader_basic::l_argc,
- test_sot_loader_basic::l_argv);
+ rclcpp::init(test_sot_loader_basic::l_argc, test_sot_loader_basic::l_argv);
}
// For each test specified in this file
- static void TearDownTestCase() {
- rclcpp::shutdown();
- }
-
+ static void TearDownTestCase() { rclcpp::shutdown(); }
+
// For each test specified by TEST_F
void SetUp() {
std::string aSotLoaderBasicName("MockSotLoaderBasic" +
std::to_string(nb_tests_));
- mockSotLoaderBasic_ptr_ = new MockSotLoaderBasic(aSotLoaderBasicName);
+ mockSotLoaderBasic_ptr_ =
+ std::make_shared<MockSotLoaderBasic>(aSotLoaderBasicName);
mockSotLoaderBasic_ptr_->initialize();
}
-
- // For each test specified by TEST_F
- void TearDown() {
- delete mockSotLoaderBasic_ptr_;
- mockSotLoaderBasic_ptr_ = nullptr;
- nb_tests_++;
- }
+ // For each test specified by TEST_F
+ void TearDown() { nb_tests_++; }
};
-TEST_F(MockSotLoaderBasicTest, TestReadingParameters) {
- mockSotLoaderBasic_ptr_->checkStateVectorMap();
-}
-
-TEST_F(MockSotLoaderBasicTest, ParseOptions) {
- mockSotLoaderBasic_ptr_->testParseOptions();
-}
-
-TEST_F(MockSotLoaderBasicTest, TestInitialization) {
- mockSotLoaderBasic_ptr_->testInitialization();
-}
-
-TEST_F(MockSotLoaderBasicTest, TestStartStopDG) {
- mockSotLoaderBasic_ptr_->test_start_stop_dg();
-}
-
-TEST_F(MockSotLoaderBasicTest, TestInitializeRosNode) {
- mockSotLoaderBasic_ptr_->testInitializeRosNode();
-}
-
-TEST_F(MockSotLoaderBasicTest, CleanUp) {
- mockSotLoaderBasic_ptr_->test_cleanup();
+TEST_F(MockSotLoaderBasicTest, UniqueTest) {
+ mockSotLoaderBasic_ptr_->UniqueTest();
}
int main(int argc, char **argv) {
::testing::InitGoogleTest(&argc, argv);
test_sot_loader_basic::l_argc = argc;
test_sot_loader_basic::l_argv = argv;
-
+
int r = RUN_ALL_TESTS();
return r;
--
GitLab