Skip to content
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

Jump to bottom

[JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available #1331

Merged
merged 2 commits into from
Nov 1, 2024
Merged

[JSB] Fix the behaviour of publishing unavailable state interfaces when they are previously available #1331

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cf7fc1f
Add test to reproduce the behaviour of https://github.com/ros-control…
saikishor Oct 30, 2024
1178e73
Add fix to solve the issue of the publishing non-existing joint_states
saikishor Oct 30, 2024
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
3 changes: 3 additions & 0 deletions joint_state_broadcaster/src/joint_state_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -209,6 +209,7 @@ controller_interface::CallbackReturn JointStateBroadcaster::on_deactivate(
const rclcpp_lifecycle::State & /*previous_state*/)
{
joint_names_.clear();
name_if_value_mapping_.clear();

return CallbackReturn::SUCCESS;
}
Expand Down Expand Up @@ -309,6 +310,8 @@ void JointStateBroadcaster::init_joint_state_msg()
void JointStateBroadcaster::init_dynamic_joint_state_msg()
{
auto & dynamic_joint_state_msg = realtime_dynamic_joint_state_publisher_->msg_;
dynamic_joint_state_msg.joint_names.clear();
dynamic_joint_state_msg.interface_values.clear();
for (const auto & name_ifv : name_if_value_mapping_)
{
const auto & name = name_ifv.first;
Expand Down
96 changes: 96 additions & 0 deletions joint_state_broadcaster/test/test_joint_state_broadcaster.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -216,6 +216,102 @@ TEST_F(JointStateBroadcasterTest, ActivateEmptyTest)
ElementsAreArray(interface_names_));
}

TEST_F(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest)
{
// publishers not initialized yet
ASSERT_FALSE(state_broadcaster_->joint_state_publisher_);
ASSERT_FALSE(state_broadcaster_->dynamic_joint_state_publisher_);

SetUpStateBroadcaster();
// configure ok
ASSERT_EQ(state_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);

ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

const size_t NUM_JOINTS = joint_names_.size();

// check interface configuration
auto cmd_if_conf = state_broadcaster_->command_interface_configuration();
ASSERT_THAT(cmd_if_conf.names, IsEmpty());
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE);
auto state_if_conf = state_broadcaster_->state_interface_configuration();
ASSERT_THAT(state_if_conf.names, IsEmpty());
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL);

// publishers initialized
ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);

// joint state initialized
const auto & joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT(joint_state_msg.name, ElementsAreArray(joint_names_));
ASSERT_THAT(joint_state_msg.position, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.velocity, SizeIs(NUM_JOINTS));
ASSERT_THAT(joint_state_msg.effort, SizeIs(NUM_JOINTS));

// dynamic joint state initialized
const auto & dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT(dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS));
ASSERT_THAT(dynamic_joint_state_msg.joint_names, ElementsAreArray(joint_names_));
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray(interface_names_));
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[1].interface_names,
ElementsAreArray(interface_names_));
ASSERT_THAT(
dynamic_joint_state_msg.interface_values[2].interface_names,
ElementsAreArray(interface_names_));

// Now deactivate and activate with only 2 set of joints and interfaces (to create as in one of
// the interface is unavailable)
ASSERT_EQ(state_broadcaster_->on_deactivate(rclcpp_lifecycle::State()), NODE_SUCCESS);
const std::vector<std::string> JOINT_NAMES = {"joint1", "joint2"};
assign_state_interfaces(JOINT_NAMES, interface_names_);

ASSERT_EQ(state_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);

const size_t NUM_JOINTS_WITH_ONE_DEACTIVATED = JOINT_NAMES.size();

// check interface configuration
cmd_if_conf = state_broadcaster_->command_interface_configuration();
ASSERT_THAT(cmd_if_conf.names, IsEmpty());
EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::NONE);
state_if_conf = state_broadcaster_->state_interface_configuration();
ASSERT_THAT(state_if_conf.names, IsEmpty());
EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::ALL);

// publishers initialized
ASSERT_TRUE(state_broadcaster_->realtime_joint_state_publisher_);
ASSERT_TRUE(state_broadcaster_->realtime_dynamic_joint_state_publisher_);

// joint state initialized
const auto & new_joint_state_msg = state_broadcaster_->realtime_joint_state_publisher_->msg_;
ASSERT_THAT(new_joint_state_msg.name, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(new_joint_state_msg.position, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
ASSERT_THAT(new_joint_state_msg.velocity, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
ASSERT_THAT(new_joint_state_msg.effort, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));

// dynamic joint state initialized
const auto & new_dynamic_joint_state_msg =
state_broadcaster_->realtime_dynamic_joint_state_publisher_->msg_;
ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
ASSERT_THAT(
new_dynamic_joint_state_msg.interface_values, SizeIs(NUM_JOINTS_WITH_ONE_DEACTIVATED));
ASSERT_THAT(new_dynamic_joint_state_msg.joint_names, ElementsAreArray(JOINT_NAMES));
ASSERT_THAT(
new_dynamic_joint_state_msg.interface_values[0].interface_names,
ElementsAreArray(interface_names_));
ASSERT_THAT(
new_dynamic_joint_state_msg.interface_values[1].interface_names,
ElementsAreArray(interface_names_));
ASSERT_THAT(
new_dynamic_joint_state_msg.interface_values[2].interface_names,
ElementsAreArray(interface_names_));
}

TEST_F(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter)
{
const std::vector<std::string> JOINT_NAMES = {};
Expand Down
1 change: 1 addition & 0 deletions joint_state_broadcaster/test/test_joint_state_broadcaster.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ class FriendJointStateBroadcaster : public joint_state_broadcaster::JointStateBr
{
FRIEND_TEST(JointStateBroadcasterTest, ConfigureErrorTest);
FRIEND_TEST(JointStateBroadcasterTest, ActivateEmptyTest);
FRIEND_TEST(JointStateBroadcasterTest, ReactivateTheControllerWithDifferentInterfacesTest);
FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameter);
FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterInvalidURDF);
FRIEND_TEST(JointStateBroadcasterTest, ActivateTestWithoutJointsParameterWithRobotDescription);
Expand Down
Loading