Skip to content

Commit

Permalink
Merge branch 'master' into enabling-not-loaded-hw-components
Browse files Browse the repository at this point in the history
  • Loading branch information
destogl authored Jan 9, 2025
2 parents 6a3870c + b329679 commit 3ac185c
Show file tree
Hide file tree
Showing 24 changed files with 2,955 additions and 191 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
public:
ControllerInterfaceBase() = default;

virtual ~ControllerInterfaceBase() = default;
virtual ~ControllerInterfaceBase();

/// Get configuration for controller's required command interfaces.
/**
Expand Down
25 changes: 25 additions & 0 deletions controller_interface/src/controller_interface_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,22 @@

namespace controller_interface
{
ControllerInterfaceBase::~ControllerInterfaceBase()
{
// check if node is initialized and we still have a valid context
if (
node_.get() &&
get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED &&
rclcpp::ok())
{
RCLCPP_DEBUG(
get_node()->get_logger(),
"Calling shutdown transition of controller node '%s' due to destruction.",
get_node()->get_name());
node_->shutdown();
}
}

return_type ControllerInterfaceBase::init(
const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
Expand Down Expand Up @@ -52,6 +68,11 @@ return_type ControllerInterfaceBase::init(
break;
case LifecycleNodeInterface::CallbackReturn::ERROR:
case LifecycleNodeInterface::CallbackReturn::FAILURE:
RCLCPP_DEBUG(
get_node()->get_logger(),
"Calling shutdown transition of controller node '%s' due to init failure.",
get_node()->get_name());
node_->shutdown();
return return_type::ERROR;
}

Expand Down Expand Up @@ -158,6 +179,10 @@ void ControllerInterfaceBase::release_interfaces()

const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() const
{
if (!node_.get())
{
throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
}
return node_->get_current_state();
}

Expand Down
13 changes: 13 additions & 0 deletions controller_interface/test/test_controller_interface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,16 +36,21 @@ TEST(TestableControllerInterface, init)
rclcpp::init(argc, argv);

TestableControllerInterface controller;
const TestableControllerInterface & const_controller = controller;

// try to get node when not initialized
ASSERT_THROW(controller.get_node(), std::runtime_error);
ASSERT_THROW(const_controller.get_node(), std::runtime_error);
ASSERT_THROW(controller.get_lifecycle_state(), std::runtime_error);

// initialize, create node
const auto node_options = controller.define_custom_node_options();
ASSERT_EQ(
controller.init(TEST_CONTROLLER_NAME, "", 10.0, "", node_options),
controller_interface::return_type::OK);
ASSERT_NO_THROW(controller.get_node());
ASSERT_NO_THROW(const_controller.get_node());
ASSERT_NO_THROW(controller.get_lifecycle_state());

// update_rate is set to controller_manager's rate
ASSERT_EQ(controller.get_update_rate(), 10u);
Expand All @@ -54,6 +59,7 @@ TEST(TestableControllerInterface, init)
controller.configure();
ASSERT_EQ(controller.get_update_rate(), 10u);

controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand All @@ -80,6 +86,8 @@ TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
// The configure should fail and the update rate should stay the same
ASSERT_EQ(controller.configure().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
ASSERT_EQ(controller.get_update_rate(), 1000u);

controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand Down Expand Up @@ -149,6 +157,7 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
ASSERT_EQ(controller.get_update_rate(), 623u);

executor->cancel();
controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand All @@ -166,6 +175,8 @@ TEST(TestableControllerInterfaceInitError, init_with_error)
controller.init(TEST_CONTROLLER_NAME, "", 100.0, "", node_options),
controller_interface::return_type::ERROR);

ASSERT_EQ(
controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
rclcpp::shutdown();
}

Expand All @@ -183,6 +194,8 @@ TEST(TestableControllerInterfaceInitFailure, init_with_failure)
controller.init(TEST_CONTROLLER_NAME, "", 50.0, "", node_options),
controller_interface::return_type::ERROR);

ASSERT_EQ(
controller.get_lifecycle_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED);
rclcpp::shutdown();
}

Expand Down
19 changes: 14 additions & 5 deletions controller_interface/test/test_controller_with_options.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ TEST(ControllerWithOption, init_with_overrides)
rclcpp::init(argc, argv);
// creates the controller
FriendControllerWithOptions controller;
EXPECT_EQ(
ASSERT_EQ(
controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
controller_interface::return_type::OK);
// checks that the node options have been updated
Expand All @@ -54,6 +54,8 @@ TEST(ControllerWithOption, init_with_overrides)
EXPECT_EQ(controller.params["parameter1"], 1.);
EXPECT_EQ(controller.params["parameter2"], 2.);
EXPECT_EQ(controller.params["parameter3"], 3.);

controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand All @@ -68,7 +70,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
controller_node_options.arguments(
{"--ros-args", "-p", "parameter_list.parameter1:=1.", "-p", "parameter_list.parameter2:=2.",
"-p", "parameter_list.parameter3:=3."});
EXPECT_EQ(
ASSERT_EQ(
controller.init("controller_name", "", 50.0, "", controller_node_options),
controller_interface::return_type::OK);
// checks that the node options have been updated
Expand All @@ -81,6 +83,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters)
EXPECT_EQ(controller.params["parameter1"], 1.);
EXPECT_EQ(controller.params["parameter2"], 2.);
EXPECT_EQ(controller.params["parameter3"], 3.);

controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand All @@ -96,7 +100,7 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
std::cerr << params_file_path << std::endl;
auto controller_node_options = controller.define_custom_node_options();
controller_node_options.arguments({"--ros-args", "--params-file", params_file_path});
EXPECT_EQ(
ASSERT_EQ(
controller.init("controller_name", "", 50.0, "", controller_node_options),
controller_interface::return_type::OK);
// checks that the node options have been updated
Expand All @@ -112,6 +116,8 @@ TEST(ControllerWithOption, init_with_node_options_arguments_parameters_file)
bool use_sim_time(true);
controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
ASSERT_FALSE(use_sim_time);

controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand All @@ -130,7 +136,7 @@ TEST(
controller_node_options.arguments(
{"--ros-args", "--params-file", params_file_path, "-p", "parameter_list.parameter1:=562.785",
"-p", "use_sim_time:=true"});
EXPECT_EQ(
ASSERT_EQ(
controller.init("controller_name", "", 50.0, "", controller_node_options),
controller_interface::return_type::OK);
// checks that the node options have been updated
Expand All @@ -146,6 +152,8 @@ TEST(
bool use_sim_time(false);
controller.get_node()->get_parameter_or("use_sim_time", use_sim_time, false);
ASSERT_TRUE(use_sim_time);

controller.get_node()->shutdown();
rclcpp::shutdown();
}

Expand All @@ -157,7 +165,7 @@ TEST(ControllerWithOption, init_without_overrides)
rclcpp::init(argc, argv);
// creates the controller
FriendControllerWithOptions controller;
EXPECT_EQ(
ASSERT_EQ(
controller.init("controller_name", "", 50.0, "", controller.define_custom_node_options()),
controller_interface::return_type::ERROR);
// checks that the node options have been updated
Expand All @@ -166,5 +174,6 @@ TEST(ControllerWithOption, init_without_overrides)
EXPECT_TRUE(node_options.automatically_declare_parameters_from_overrides());
// checks that no parameter has been declared from overrides
EXPECT_EQ(controller.params.size(), 0u);

rclcpp::shutdown();
}
3 changes: 2 additions & 1 deletion controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -255,7 +255,8 @@ ControllerManager::ControllerManager(
chainable_loader_(
std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
cm_node_options_(options)
cm_node_options_(options),
robot_description_(urdf)
{
initialize_parameters();
resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(
Expand Down
5 changes: 5 additions & 0 deletions hardware_interface/src/component_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,12 @@
#include <unordered_map>
#include <vector>

#include "rclcpp/version.h"
#if RCLCPP_VERSION_GTE(29, 0, 0)
#include "urdf/model.hpp"
#else
#include "urdf/model.h"
#endif

#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/hardware_info.hpp"
Expand Down
33 changes: 33 additions & 0 deletions joint_limits/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -44,14 +44,28 @@ target_include_directories(joint_limiter_interface PUBLIC
)
ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})

add_library(joint_limits_helpers SHARED
src/joint_limits_helpers.cpp
)
target_compile_features(joint_limits_helpers PUBLIC cxx_std_17)
target_include_directories(joint_limits_helpers PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/joint_limits>
)
ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})

add_library(joint_saturation_limiter SHARED
src/joint_saturation_limiter.cpp
src/joint_range_limiter.cpp
src/joint_soft_limiter.cpp
)
target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17)
target_include_directories(joint_saturation_limiter PUBLIC
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include/joint_limits>
)
target_link_libraries(joint_saturation_limiter PUBLIC joint_limits_helpers)

ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})

pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml)
Expand Down Expand Up @@ -91,6 +105,24 @@ if(BUILD_TESTING)
rclcpp
)

ament_add_gmock(test_joint_range_limiter test/test_joint_range_limiter.cpp)
target_include_directories(test_joint_range_limiter PRIVATE include)
target_link_libraries(test_joint_range_limiter joint_limiter_interface)
ament_target_dependencies(
test_joint_range_limiter
pluginlib
rclcpp
)

ament_add_gmock(test_joint_soft_limiter test/test_joint_soft_limiter.cpp)
target_include_directories(test_joint_soft_limiter PRIVATE include)
target_link_libraries(test_joint_soft_limiter joint_limiter_interface)
ament_target_dependencies(
test_joint_soft_limiter
pluginlib
rclcpp
)

endif()

install(
Expand All @@ -101,6 +133,7 @@ install(TARGETS
joint_limits
joint_limiter_interface
joint_saturation_limiter
joint_limits_helpers
EXPORT export_joint_limits
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
Expand Down
66 changes: 66 additions & 0 deletions joint_limits/include/joint_limits/data_structures.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
// Copyright 2024 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/// \author Sai Kishor Kothakota

#ifndef JOINT_LIMITS__DATA_STRUCTURES_HPP_
#define JOINT_LIMITS__DATA_STRUCTURES_HPP_

#include <memory>
#include <optional>
#include <string>

namespace joint_limits
{

struct JointControlInterfacesData
{
std::string joint_name;
std::optional<double> position = std::nullopt;
std::optional<double> velocity = std::nullopt;
std::optional<double> effort = std::nullopt;
std::optional<double> acceleration = std::nullopt;
std::optional<double> jerk = std::nullopt;

bool has_data() const
{
return has_position() || has_velocity() || has_effort() || has_acceleration() || has_jerk();
}

bool has_position() const { return position.has_value(); }

bool has_velocity() const { return velocity.has_value(); }

bool has_effort() const { return effort.has_value(); }

bool has_acceleration() const { return acceleration.has_value(); }

bool has_jerk() const { return jerk.has_value(); }
};

struct JointInterfacesCommandLimiterData
{
std::string joint_name;
JointControlInterfacesData actual;
JointControlInterfacesData command;
JointControlInterfacesData prev_command;
JointControlInterfacesData limited;

bool has_actual_data() const { return actual.has_data(); }

bool has_command_data() const { return command.has_data(); }
};

} // namespace joint_limits
#endif // JOINT_LIMITS__DATA_STRUCTURES_HPP_
Loading

0 comments on commit 3ac185c

Please sign in to comment.