Skip to content

Remove lifecycle node controllers #124

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
bmagyar merged 8 commits into from
Dec 14, 2020
Merged

Remove lifecycle node controllers #124

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
a4149d1
Remove lifecycle node from joint_state_controller
Dec 9, 2020
8eaf083
Remove lifecycle node from joint_trajectory_controller
Dec 9, 2020
da5a604
Remove lifecycle node from forward_command_controller
Dec 9, 2020
8ca2bbc
Merge branch 'master' into remove-lifecycle-node
bmagyar Dec 12, 2020
2213157
Remove additional checks
bmagyar Dec 13, 2020
7691468
Remove lifecycle include
v-lopez Dec 13, 2020
5f3bcf3
Adjust test & add back node nullptr check
bmagyar Dec 14, 2020
d7c87cb
use get_node()
bmagyar Dec 14, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,7 +124,7 @@ DiffDriveController::init(

controller_interface::return_type DiffDriveController::update()
{
auto logger = lifecycle_node_->get_logger();
auto logger = node_->get_logger();
if (lifecycle_node_->get_current_state().id() == State::PRIMARY_STATE_INACTIVE) {
if (!is_halted) {
halt();
Expand All @@ -133,7 +133,7 @@ controller_interface::return_type DiffDriveController::update()
return controller_interface::return_type::SUCCESS;
}

const auto current_time = lifecycle_node_->get_clock()->now();
const auto current_time = node_->get_clock()->now();
double & linear_command = received_velocity_msg_ptr_->twist.linear.x;
double & angular_command = received_velocity_msg_ptr_->twist.angular.z;

Expand Down Expand Up @@ -255,7 +255,7 @@ controller_interface::return_type DiffDriveController::update()

CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &)
{
auto logger = lifecycle_node_->get_logger();
auto logger = node_->get_logger();

// update parameters
left_wheel_names_ = lifecycle_node_->get_parameter("left_wheel_names").as_string_array();
Expand Down Expand Up @@ -399,7 +399,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &
const std::shared_ptr<Twist> msg) -> void {
if (!subscriber_is_active_) {
RCLCPP_WARN(
lifecycle_node_->get_logger(),
node_->get_logger(),
"Can't accept new commands. subscriber is inactive");
return;
}
Expand Down Expand Up @@ -448,7 +448,7 @@ CallbackReturn DiffDriveController::on_configure(const rclcpp_lifecycle::State &
odometry_transform_message.transforms.front().header.frame_id = odom_params_.odom_frame_id;
odometry_transform_message.transforms.front().child_frame_id = odom_params_.base_frame_id;

previous_update_timestamp_ = lifecycle_node_->get_clock()->now();
previous_update_timestamp_ = node_->get_clock()->now();
set_op_mode(hardware_interface::OperationMode::INACTIVE);
return CallbackReturn::SUCCESS;
}
Expand All @@ -465,7 +465,7 @@ CallbackReturn DiffDriveController::on_activate(const rclcpp_lifecycle::State &)
}

RCLCPP_INFO(
lifecycle_node_->get_logger(), "Lifecycle subscriber and publisher are currently active.");
node_->get_logger(), "Lifecycle subscriber and publisher are currently active.");
return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -549,7 +549,7 @@ CallbackReturn DiffDriveController::configure_side(
std::vector<WheelHandle> & registered_handles,
hardware_interface::RobotHardware & robot_hardware)
{
auto logger = lifecycle_node_->get_logger();
auto logger = node_->get_logger();

if (wheel_names.empty()) {
std::stringstream ss;
Expand Down
22 changes: 11 additions & 11 deletions forward_command_controller/src/forward_command_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,34 +38,34 @@ CallbackReturn ForwardCommandController::on_configure(
const rclcpp_lifecycle::State & /*previous_state*/)
{
rclcpp::Parameter joints_param, interface_param;
if (!lifecycle_node_->get_parameter("joints", joint_names_)) {
RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'joints' parameter not set");
if (!node_->get_parameter("joints", joint_names_)) {
RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'joints' parameter not set");
return CallbackReturn::ERROR;
}

if (joint_names_.empty()) {
RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'joints' parameter was empty");
RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'joints' parameter was empty");
return CallbackReturn::ERROR;
}

if (!lifecycle_node_->get_parameter("interface_name", interface_name_)) {
RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'interface_name' parameter not set");
if (!node_->get_parameter("interface_name", interface_name_)) {
RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'interface_name' parameter not set");
return CallbackReturn::ERROR;
}

if (interface_name_.empty()) {
RCLCPP_ERROR_STREAM(get_lifecycle_node()->get_logger(), "'interface_name' parameter was empty");
RCLCPP_ERROR_STREAM(get_node()->get_logger(), "'interface_name' parameter was empty");
return CallbackReturn::ERROR;
}

joints_command_subscriber_ = lifecycle_node_->create_subscription<CmdType>(
joints_command_subscriber_ = node_->create_subscription<CmdType>(
"~/commands", rclcpp::SystemDefaultsQoS(),
[this](const CmdType::SharedPtr msg)
{
rt_command_ptr_.writeFromNonRT(msg);
});

RCLCPP_INFO_STREAM(get_lifecycle_node()->get_logger(), "configure successful");
RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful");
return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -120,7 +120,7 @@ CallbackReturn ForwardCommandController::on_activate(
ordered_interfaces) || command_interfaces_.size() != ordered_interfaces.size())
{
RCLCPP_ERROR(
lifecycle_node_->get_logger(),
node_->get_logger(),
"Expected %u position command interfaces, got %u",
joint_names_.size(), ordered_interfaces.size());
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
Expand All @@ -146,8 +146,8 @@ controller_interface::return_type ForwardCommandController::update()

if ((*joint_commands)->data.size() != command_interfaces_.size()) {
RCLCPP_ERROR_STREAM_THROTTLE(
get_lifecycle_node()->get_logger(),
*lifecycle_node_->get_clock(), 1000,
get_node()->get_logger(),
*node_->get_clock(), 1000,
"command size (" << (*joint_commands)->data.size() <<
") does not match number of interfaces (" <<
command_interfaces_.size() << ")");
Expand Down
56 changes: 28 additions & 28 deletions forward_command_controller/test/test_forward_command_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ void ForwardCommandControllerTest::SetUpController()
TEST_F(ForwardCommandControllerTest, JointsParameterNotSet)
{
SetUpController();
controller_->lifecycle_node_->declare_parameter("interface_name", "dummy");
controller_->node_->declare_parameter("interface_name", "dummy");

// configure failed, 'joints' parameter not set
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
Expand All @@ -97,21 +97,21 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterNotSet)
SetUpController();

// configure failed, 'interface_name' parameter not set
controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>()));
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
controller_->lifecycle_node_->declare_parameter("interface_name", "");
controller_->node_->declare_parameter("interface_name", "");
}

TEST_F(ForwardCommandControllerTest, JointsParameterIsEmpty)
{
SetUpController();

controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>()));
controller_->lifecycle_node_->declare_parameter("interface_name", "");
controller_->node_->declare_parameter("interface_name", "");

// configure failed, 'joints' is empty
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
Expand All @@ -122,10 +122,10 @@ TEST_F(ForwardCommandControllerTest, InterfaceParameterEmpty)
SetUpController();

// configure failed, 'interface_name' paremeter not set
controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "");
controller_->node_->declare_parameter("interface_name", "");

// configure failed, 'interface_name' is empty
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
Expand All @@ -135,10 +135,10 @@ TEST_F(ForwardCommandControllerTest, ConfigureParamsSuccess)
{
SetUpController();

controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");

// configure successful
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
Expand All @@ -148,16 +148,16 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongJointsNamesFails)
{
SetUpController();

controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint4"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");

// activate failed, 'joint4' is not a valid joint name for the hardware
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);

auto result = controller_->lifecycle_node_->set_parameter(
auto result = controller_->node_->set_parameter(
rclcpp::Parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2"})));
Expand All @@ -172,10 +172,10 @@ TEST_F(ForwardCommandControllerTest, ActivateWithWrongInterfaceNameFails)
{
SetUpController();

controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint3"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "acceleration");
controller_->node_->declare_parameter("interface_name", "acceleration");

// activate failed, 'joint4' not in interfaces
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
Expand All @@ -186,10 +186,10 @@ TEST_F(ForwardCommandControllerTest, ActivateSuccess)
{
SetUpController();

controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint3"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");

ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
Expand All @@ -200,10 +200,10 @@ TEST_F(ForwardCommandControllerTest, CommandSuccessTest)
SetUpController();

// configure controller
controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint3"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

// update successful though no command has been send yet
Expand Down Expand Up @@ -234,10 +234,10 @@ TEST_F(ForwardCommandControllerTest, WrongCommandCheckTest)
SetUpController();

// configure controller
controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint3"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

// send command with wrong numnber of joints
Expand All @@ -260,10 +260,10 @@ TEST_F(ForwardCommandControllerTest, NoCommandCheckTest)
SetUpController();

// configure controller
controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint3"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");
ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);

// update successful, no command received yet
Expand All @@ -279,27 +279,27 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
{
SetUpController();

controller_->lifecycle_node_->declare_parameter(
controller_->node_->declare_parameter(
"joints",
rclcpp::ParameterValue(std::vector<std::string>{"joint1", "joint2", "joint3"}));
controller_->lifecycle_node_->declare_parameter("interface_name", "position");
controller_->node_->declare_parameter("interface_name", "position");

// default values
ASSERT_EQ(joint_1_pos_cmd_.get_value(), 1.1);
ASSERT_EQ(joint_2_pos_cmd_.get_value(), 2.1);
ASSERT_EQ(joint_3_pos_cmd_.get_value(), 3.1);

auto node_state = controller_->get_lifecycle_node()->configure();
auto node_state = controller_->configure();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

node_state = controller_->get_lifecycle_node()->activate();
node_state = controller_->activate();
ASSERT_EQ(node_state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

// send a new command
rclcpp::Node test_node("test_node");
auto command_pub = test_node.create_publisher<std_msgs::msg::Float64MultiArray>(
std::string(
controller_->get_lifecycle_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS());
controller_->get_node()->get_name()) + "/commands", rclcpp::SystemDefaultsQoS());
std_msgs::msg::Float64MultiArray command_msg;
command_msg.data = {10.0, 20.0, 30.0};
command_pub->publish(command_msg);
Expand All @@ -308,7 +308,7 @@ TEST_F(ForwardCommandControllerTest, CommandCallbackTest)
ASSERT_EQ(wait_for(controller_->joints_command_subscriber_), rclcpp::WaitResultKind::Ready);

// process callbacks
rclcpp::spin_some(controller_->get_lifecycle_node()->get_node_base_interface());
rclcpp::spin_some(controller_->get_node()->get_node_base_interface());

// update successful
ASSERT_EQ(controller_->update(), controller_interface::return_type::SUCCESS);
Expand Down
4 changes: 2 additions & 2 deletions joint_state_controller/include/joint_state_controller/joint_state_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,14 +67,14 @@ class JointStateController : public controller_interface::ControllerInterface
// For the JointState message,
// we store the name of joints with compatible interfaces
std::vector<std::string> joint_names_;
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::JointState>>
std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>>
joint_state_publisher_;
sensor_msgs::msg::JointState joint_state_msg_;

// For the DynamicJointState format, we use a map to buffer values in for easier lookup
// This allows to preserve whatever order or names/interfaces were initialized.
std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<control_msgs::msg::DynamicJointState>>
std::shared_ptr<rclcpp::Publisher<control_msgs::msg::DynamicJointState>>
dynamic_joint_state_publisher_;
control_msgs::msg::DynamicJointState dynamic_joint_state_msg_;
};
Expand Down
27 changes: 6 additions & 21 deletions joint_state_controller/src/joint_state_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@

#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "lifecycle_msgs/msg/state.hpp"

@destogl destogl Dec 13, 2020

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Also remove this line please.

@v-lopez v-lopez Dec 13, 2020

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

#include "rclcpp/clock.hpp"
#include "rclcpp/qos.hpp"
#include "rclcpp/qos_event.hpp"
Expand Down Expand Up @@ -67,11 +68,11 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
JointStateController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
try {
joint_state_publisher_ = lifecycle_node_->create_publisher<sensor_msgs::msg::JointState>(
joint_state_publisher_ = node_->create_publisher<sensor_msgs::msg::JointState>(
"joint_states", rclcpp::SystemDefaultsQoS());

dynamic_joint_state_publisher_ =
lifecycle_node_->create_publisher<control_msgs::msg::DynamicJointState>(
node_->create_publisher<control_msgs::msg::DynamicJointState>(
"dynamic_joint_states", rclcpp::SystemDefaultsQoS());
} catch (...) {
return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
Expand All @@ -89,18 +90,12 @@ JointStateController::on_activate(const rclcpp_lifecycle::State & /*previous_sta
init_joint_state_msg();
init_dynamic_joint_state_msg();

joint_state_publisher_->on_activate();
dynamic_joint_state_publisher_->on_activate();

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
JointStateController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/)
{
joint_state_publisher_->on_deactivate();
dynamic_joint_state_publisher_->on_deactivate();

return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -194,24 +189,14 @@ JointStateController::update()
name_if_value_mapping_[state_interface.get_name()][state_interface.get_interface_name()] =
state_interface.get_value();
RCLCPP_DEBUG(
get_lifecycle_node()->get_logger(), "%s/%s: %f\n",
get_node()->get_logger(), "%s/%s: %f\n",
state_interface.get_name().c_str(),
state_interface.get_interface_name().c_str(),
state_interface.get_value());
}

if (!joint_state_publisher_->is_activated()) {
RCUTILS_LOG_WARN_ONCE_NAMED("publisher", "joint state publisher is not activated");
return controller_interface::return_type::ERROR;
}

if (!dynamic_joint_state_publisher_->is_activated()) {
RCUTILS_LOG_WARN_ONCE_NAMED("publisher", "dynamic joint state publisher is not activated");
return controller_interface::return_type::ERROR;
}

joint_state_msg_.header.stamp = lifecycle_node_->get_clock()->now();
dynamic_joint_state_msg_.header.stamp = lifecycle_node_->get_clock()->now();
joint_state_msg_.header.stamp = node_->get_clock()->now();
dynamic_joint_state_msg_.header.stamp = node_->get_clock()->now();

// update joint state message and dynamic joint state message
for (auto i = 0ul; i < joint_names_.size(); ++i) {
Expand Down
Loading