Skip to content

Commit babecd8

Browse files
TedVanderfeenTedVanderfeen
authored
Publish JointState commands for velocity-only and effort-only interfaces (#118)
1 parent 5c57e3d commit babecd8

2 files changed

Lines changed: 187 additions & 1 deletion

File tree

‎joint_state_topic_hardware_interface/src/joint_state_topic_hardware_interface.cpp‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -182,7 +182,10 @@ hardware_interface::return_type JointStateTopicSystem::write(const rclcpp::Time&
182182
{
183183
for (const auto& interface : joints[i].command_interfaces)
184184
{
185-
if (interface.name != hardware_interface::HW_IF_POSITION)
185+
const bool supported_command_interface = interface.name == hardware_interface::HW_IF_POSITION ||
186+
interface.name == hardware_interface::HW_IF_VELOCITY ||
187+
interface.name == hardware_interface::HW_IF_EFFORT;
188+
if (!supported_command_interface)
186189
{
187190
continue;
188191
}

‎joint_state_topic_hardware_interface/test/joint_state_topic_hardware_interface_test.cpp‎

Lines changed: 183 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -393,6 +393,189 @@ TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only)
393393
EXPECT_EQ(j2_v_c.get_optional().value(), 0.14);
394394
}
395395

396+
TEST_F(TestTopicBasedSystem, topic_based_system_2dof_position_only_publishes_commands)
397+
{
398+
sensor_msgs::msg::JointState::SharedPtr command_msg;
399+
auto command_subscriber = node_->create_subscription<sensor_msgs::msg::JointState>(
400+
"/topic_based_joint_commands", rclcpp::QoS(1),
401+
[&command_msg](const sensor_msgs::msg::JointState::SharedPtr msg) { command_msg = msg; });
402+
executor_->add_node(node_);
403+
404+
const std::string hardware_system_2dof_topic_based =
405+
R"(
406+
<ros2_control name="JointStateTopicBasedSystem2dof" type="system">
407+
<hardware>
408+
<plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
409+
<param name="joint_commands_topic">/topic_based_joint_commands</param>
410+
<param name="joint_states_topic">/topic_based_custom_joint_states</param>
411+
</hardware>
412+
<joint name="joint1">
413+
<command_interface name="position"/>
414+
<state_interface name="position"/>
415+
</joint>
416+
<joint name="joint2">
417+
<command_interface name="position"/>
418+
<state_interface name="position"/>
419+
</joint>
420+
</ros2_control>
421+
)";
422+
auto urdf =
423+
ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail;
424+
425+
init_rm(urdf);
426+
427+
// Activate components to get all interfaces available
428+
activate_components(*rm_, { "JointStateTopicBasedSystem2dof" });
429+
430+
hardware_interface::LoanedCommandInterface j1_p_c = rm_->claim_command_interface("joint1/position");
431+
hardware_interface::LoanedCommandInterface j2_p_c = rm_->claim_command_interface("joint2/position");
432+
433+
ASSERT_TRUE(j1_p_c.set_value(0.12));
434+
ASSERT_TRUE(j2_p_c.set_value(0.14));
435+
436+
int wait_count = 0;
437+
while (node_->count_publishers("/topic_based_joint_commands") == 0 && wait_count < 5)
438+
{
439+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
440+
++wait_count;
441+
}
442+
ASSERT_GT(node_->count_publishers("/topic_based_joint_commands"), 0u);
443+
444+
hardware_interface::return_type ret;
445+
ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result);
446+
ASSERT_EQ(ret, hardware_interface::return_type::OK);
447+
448+
wait_for_msg(std::chrono::milliseconds{ 100 });
449+
450+
ASSERT_NE(command_msg, nullptr);
451+
EXPECT_THAT(command_msg->name, ::testing::ElementsAre("joint1", "joint2"));
452+
EXPECT_THAT(command_msg->position, ::testing::ElementsAre(0.12, 0.14));
453+
EXPECT_TRUE(command_msg->velocity.empty());
454+
EXPECT_TRUE(command_msg->effort.empty());
455+
}
456+
457+
TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only_publishes_commands)
458+
{
459+
sensor_msgs::msg::JointState::SharedPtr command_msg;
460+
auto command_subscriber = node_->create_subscription<sensor_msgs::msg::JointState>(
461+
"/topic_based_joint_commands", rclcpp::QoS(1),
462+
[&command_msg](const sensor_msgs::msg::JointState::SharedPtr msg) { command_msg = msg; });
463+
executor_->add_node(node_);
464+
465+
const std::string hardware_system_2dof_topic_based =
466+
R"(
467+
<ros2_control name="JointStateTopicBasedSystem2dof" type="system">
468+
<hardware>
469+
<plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
470+
<param name="joint_commands_topic">/topic_based_joint_commands</param>
471+
<param name="joint_states_topic">/topic_based_custom_joint_states</param>
472+
</hardware>
473+
<joint name="joint1">
474+
<command_interface name="velocity"/>
475+
<state_interface name="velocity"/>
476+
</joint>
477+
<joint name="joint2">
478+
<command_interface name="velocity"/>
479+
<state_interface name="velocity"/>
480+
</joint>
481+
</ros2_control>
482+
)";
483+
auto urdf =
484+
ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail;
485+
486+
init_rm(urdf);
487+
488+
// Activate components to get all interfaces available
489+
activate_components(*rm_, { "JointStateTopicBasedSystem2dof" });
490+
491+
hardware_interface::LoanedCommandInterface j1_v_c = rm_->claim_command_interface("joint1/velocity");
492+
hardware_interface::LoanedCommandInterface j2_v_c = rm_->claim_command_interface("joint2/velocity");
493+
494+
ASSERT_TRUE(j1_v_c.set_value(0.12));
495+
ASSERT_TRUE(j2_v_c.set_value(0.14));
496+
497+
int wait_count = 0;
498+
while (node_->count_publishers("/topic_based_joint_commands") == 0 && wait_count < 5)
499+
{
500+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
501+
++wait_count;
502+
}
503+
ASSERT_GT(node_->count_publishers("/topic_based_joint_commands"), 0u);
504+
505+
hardware_interface::return_type ret;
506+
ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result);
507+
ASSERT_EQ(ret, hardware_interface::return_type::OK);
508+
509+
wait_for_msg(std::chrono::milliseconds{ 100 });
510+
511+
ASSERT_NE(command_msg, nullptr);
512+
EXPECT_THAT(command_msg->name, ::testing::ElementsAre("joint1", "joint2"));
513+
EXPECT_TRUE(command_msg->position.empty());
514+
EXPECT_THAT(command_msg->velocity, ::testing::ElementsAre(0.12, 0.14));
515+
EXPECT_TRUE(command_msg->effort.empty());
516+
}
517+
518+
TEST_F(TestTopicBasedSystem, topic_based_system_2dof_effort_only_publishes_commands)
519+
{
520+
sensor_msgs::msg::JointState::SharedPtr command_msg;
521+
auto command_subscriber = node_->create_subscription<sensor_msgs::msg::JointState>(
522+
"/topic_based_joint_commands", rclcpp::QoS(1),
523+
[&command_msg](const sensor_msgs::msg::JointState::SharedPtr msg) { command_msg = msg; });
524+
executor_->add_node(node_);
525+
526+
const std::string hardware_system_2dof_topic_based =
527+
R"(
528+
<ros2_control name="JointStateTopicBasedSystem2dof" type="system">
529+
<hardware>
530+
<plugin>joint_state_topic_hardware_interface/JointStateTopicSystem</plugin>
531+
<param name="joint_commands_topic">/topic_based_joint_commands</param>
532+
<param name="joint_states_topic">/topic_based_custom_joint_states</param>
533+
</hardware>
534+
<joint name="joint1">
535+
<command_interface name="effort"/>
536+
<state_interface name="effort"/>
537+
</joint>
538+
<joint name="joint2">
539+
<command_interface name="effort"/>
540+
<state_interface name="effort"/>
541+
</joint>
542+
</ros2_control>
543+
)";
544+
auto urdf =
545+
ros2_control_test_assets::urdf_head + hardware_system_2dof_topic_based + ros2_control_test_assets::urdf_tail;
546+
547+
init_rm(urdf);
548+
549+
// Activate components to get all interfaces available
550+
activate_components(*rm_, { "JointStateTopicBasedSystem2dof" });
551+
552+
hardware_interface::LoanedCommandInterface j1_e_c = rm_->claim_command_interface("joint1/effort");
553+
hardware_interface::LoanedCommandInterface j2_e_c = rm_->claim_command_interface("joint2/effort");
554+
555+
ASSERT_TRUE(j1_e_c.set_value(0.12));
556+
ASSERT_TRUE(j2_e_c.set_value(0.14));
557+
558+
int wait_count = 0;
559+
while (node_->count_publishers("/topic_based_joint_commands") == 0 && wait_count < 5)
560+
{
561+
std::this_thread::sleep_for(std::chrono::milliseconds(100));
562+
++wait_count;
563+
}
564+
ASSERT_GT(node_->count_publishers("/topic_based_joint_commands"), 0u);
565+
566+
hardware_interface::return_type ret;
567+
ASSERT_NO_THROW(ret = rm_->write(TIME, PERIOD).result);
568+
ASSERT_EQ(ret, hardware_interface::return_type::OK);
569+
570+
wait_for_msg(std::chrono::milliseconds{ 100 });
571+
572+
ASSERT_NE(command_msg, nullptr);
573+
EXPECT_THAT(command_msg->name, ::testing::ElementsAre("joint1", "joint2"));
574+
EXPECT_TRUE(command_msg->position.empty());
575+
EXPECT_TRUE(command_msg->velocity.empty());
576+
EXPECT_THAT(command_msg->effort, ::testing::ElementsAre(0.12, 0.14));
577+
}
578+
396579
TEST_F(TestTopicBasedSystem, topic_based_system_2dof_velocity_only_inconsistent_topic)
397580
{
398581
const std::string hardware_system_2dof_topic_based =

0 commit comments

Comments
 (0)