Skip to content

Commit

Permalink
Test fixes.
Browse files Browse the repository at this point in the history
  • Loading branch information
@destogl
destogl committed Aug 15, 2024
1 parent bc75c66 commit 9b96aa2
Show file tree
Hide file tree
Showing 5 changed files with 79 additions and 107 deletions.
4 changes: 4 additions & 0 deletions controller_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,11 +169,15 @@ if(BUILD_TESTING)
ament_add_gmock(
test_spawner_unspawner
test/test_spawner_unspawner.cpp
TIMEOUT 120
)
target_include_directories(test_spawner_unspawner PRIVATE include)
target_link_libraries(test_spawner_unspawner ${PROJECT_NAME} test_controller)
ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets)

install(FILES test/test_controller_spawner_with_type.yaml
DESTINATION test)

ament_add_gmock(
test_hardware_management_srvs
test/test_hardware_management_srvs.cpp
Expand Down
27 changes: 26 additions & 1 deletion controller_manager/controller_manager/spawner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
import sys
import time
import warnings
import yaml

from controller_manager import (
configure_controller,
Expand Down Expand Up @@ -79,6 +80,24 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time
return any(c.name == controller_name for c in controllers)


def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name):
with open(parameter_file) as f:
namespaced_controller = (
controller_name if namespace == "/" else f"{namespace}/{controller_name}"
)
parameters = yaml.safe_load(f)
if namespaced_controller in parameters:
value = parameters[namespaced_controller]
if not isinstance(value, dict) or "ros__parameters" not in value:
raise RuntimeError(
f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}"
)
if parameter_name in parameters[namespaced_controller]["ros__parameters"]:
return parameters[namespaced_controller]["ros__parameters"][parameter_name]
else:
return None


def main(args=None):

rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO)
Expand Down Expand Up @@ -152,7 +171,6 @@ def main(args=None):
controller_manager_name = args.controller_manager
controller_namespace = args.namespace
param_file = args.param_file
controller_type = args.controller_type
controller_manager_timeout = args.controller_manager_timeout

if param_file and not os.path.isfile(param_file):
Expand Down Expand Up @@ -182,6 +200,13 @@ def main(args=None):
+ bcolors.ENDC
)
else:
controller_type = (
None
if param_file is None
else get_parameter_from_param_file(
controller_name, spawner_namespace, param_file, "type"
)
)
if controller_type:
parameter = Parameter()
parameter.name = prefixed_controller_name + ".type"
Expand Down
27 changes: 20 additions & 7 deletions controller_manager/test/controller_manager_test_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,15 +67,17 @@ class ControllerManagerFixture : public ::testing::Test
public:
explicit ControllerManagerFixture(
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf,
const bool & pass_urdf_as_parameter = false)
const bool & pass_urdf_as_parameter = false,
const std::string & cm_namespace = "")
: robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter)
{
executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
// We want to be able to create a ResourceManager where no urdf file has been passed to
if (robot_description_.empty())
{
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME,
cm_namespace);
}
else
{
Expand All @@ -85,7 +87,7 @@ class ControllerManagerFixture : public ::testing::Test
{
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(robot_description_, true, true),
executor_, TEST_CM_NAME);
executor_, TEST_CM_NAME, cm_namespace);
}
else
{
Expand All @@ -94,11 +96,10 @@ class ControllerManagerFixture : public ::testing::Test

// this is just a workaround to skip passing
cm_ = std::make_shared<CtrlMgr>(
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME,
cm_namespace);
// mimic topic call
auto msg = std_msgs::msg::String();
msg.data = robot_description_;
cm_->robot_description_callback(msg);
pass_robot_description_to_cm_and_rm(robot_description_);
}
}
}
Expand Down Expand Up @@ -134,6 +135,17 @@ class ControllerManagerFixture : public ::testing::Test
}
}

void pass_robot_description_to_cm_and_rm(
const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf)
{
// TODO(Manuel) : passing via topic not working in test setup, tested cm does
// not receive msg. Have to check this...
// this is just a workaround to skip passing - mimic topic call
auto msg = std_msgs::msg::String();
msg.data = robot_description;
cm_->robot_description_callback(msg);
}

void switch_test_controllers(
const std::vector<std::string> & start_controllers,
const std::vector<std::string> & stop_controllers, const int strictness,
Expand All @@ -157,6 +169,7 @@ class ControllerManagerFixture : public ::testing::Test
std::thread updater_;
bool run_updater_;
const std::string robot_description_;
rclcpp::Time time_;
const bool pass_urdf_as_parameter_;
};

Expand Down
27 changes: 27 additions & 0 deletions controller_manager/test/test_controller_spawner_with_type.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,27 @@
ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint0"]

chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]

ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]

/foo_namespace/ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_controller"
joint_names: ["joint1"]

/foo_namespace/chainable_ctrl_with_parameters_and_type:
ros__parameters:
type: "controller_manager/test_chainable_controller"
joint_names: ["joint1"]

/foo_namespace/ctrl_with_parameters_and_no_type:
ros__parameters:
joint_names: ["joint2"]
101 changes: 2 additions & 99 deletions controller_manager/test/test_spawner_unspawner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_chainable_controller/test_chainable_controller.hpp"
#include "test_controller/test_controller.hpp"

using ::testing::_;
Expand Down Expand Up @@ -222,51 +223,11 @@ TEST_F(TestLoadController, spawner_test_type_in_arg)
std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)),
0);

<<<<<<< HEAD
ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
auto ctrl_2 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);
=======
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);

auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
ctrl_with_parameters_and_type.c->get_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1];
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.info.type,
test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(
chain_ctrl_with_parameters_and_type.c->get_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

EXPECT_EQ(
call_spawner(
"ctrl_with_parameters_and_no_type -c test_controller_manager --controller-manager-timeout "
"1.0 -p " +
test_file_path),
256);
// Will still be same as the current call will fail
ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul);

auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type");
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562))
}

TEST_F(TestLoadController, unload_on_kill)
Expand All @@ -284,63 +245,6 @@ TEST_F(TestLoadController, unload_on_kill)

ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul);
}
<<<<<<< HEAD
=======

TEST_F(TestLoadController, spawner_test_fallback_controllers)
{
const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") +
"/test/test_controller_spawner_with_fallback_controllers.yaml";

cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME));
cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME));

ControllerManagerRunner cm_runner(this);
EXPECT_EQ(
call_spawner(
"ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 "
"-p " +
test_file_path),
0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}

// Try to spawn now the controller with fallback controllers inside the yaml
EXPECT_EQ(
call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0);

ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul);
{
auto ctrl_1 = cm_->get_loaded_controllers()[0];
ASSERT_EQ(ctrl_1.info.name, "ctrl_1");
ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5"));
ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_2 = cm_->get_loaded_controllers()[1];
ASSERT_EQ(ctrl_2.info.name, "ctrl_2");
ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(
ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8"));
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

auto ctrl_3 = cm_->get_loaded_controllers()[2];
ASSERT_EQ(ctrl_3.info.name, "ctrl_3");
ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9"));
ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
}

class TestLoadControllerWithoutRobotDescription
: public ControllerManagerFixture<controller_manager::ControllerManager>
Expand Down Expand Up @@ -429,7 +333,7 @@ class TestLoadControllerWithNamespacedCM
public:
TestLoadControllerWithNamespacedCM()
: ControllerManagerFixture<controller_manager::ControllerManager>(
ros2_control_test_assets::minimal_robot_urdf, "foo_namespace")
ros2_control_test_assets::minimal_robot_urdf, false, "foo_namespace")
{
}

Expand Down Expand Up @@ -699,4 +603,3 @@ TEST_F(
ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME);
ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
}
>>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562))

0 comments on commit 9b96aa2

Please sign in to comment.