Skip to content
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -658,6 +658,12 @@ const auto hardware_resources =
<state_interface name="velocity"/>
<command_interface name="max_velocity" />
</joint>
<transmission name="ros2_control_transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint1" role="joint1">
<mechanical_reduction>325.949</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
<ros2_control name="TestSensorHardware" type="sensor">
<hardware>
Expand Down Expand Up @@ -692,9 +698,77 @@ const auto hardware_resources =
<command_interface name="max_tcp_jerk"/>
<state_interface name="max_tcp_jerk"/>
</gpio>
<transmission name="differential_transmission">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint2_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint3_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint2" role="joint1">
<offset>0.5</offset>
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint3" role="joint2">
<offset>-0.5</offset>
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
<transmission name="four_bar_transmission">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint2_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint3_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint2" role="joint1">
<offset>0.5</offset>
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint3" role="joint2">
<offset>-0.5</offset>
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
)";

const auto minimal_robot_transmissions =
R"(
<transmission name="minimal_transmission1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="minimal_motor1">
<mechanicalReduction>1</mechanicalReduction>
<hardwareInterface>PositionJointInterface</hardwareInterface>
</actuator>
</transmission>
<transmission name="minimal_transmission2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint2">
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</joint>
<actuator name="minimal_motor2">
<mechanicalReduction>60</mechanicalReduction>
<hardwareInterface>VelocityJointInterface</hardwareInterface>
</actuator>
</transmission>
)";

const auto minimal_robot_transmissions_wrong =
R"(
<transmission name="minimal_transmission1">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="minimal_motor1">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
)";

const auto hardware_resources_with_disabled_limits =
R"(
<ros2_control name="TestActuatorHardware" type="actuator">
Expand Down Expand Up @@ -2228,7 +2302,8 @@ const auto diff_drive_robot_sdf =
)";

const auto minimal_robot_urdf =
std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail);
std::string(urdf_head) + std::string(hardware_resources) +
std::string(minimal_robot_transmissions) + std::string(urdf_tail);
const auto minimal_robot_urdf_no_limits = std::string(urdf_head_continuous_missing_limits) +
std::string(hardware_resources) + std::string(urdf_tail);
const auto minimal_async_robot_urdf =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,56 +56,36 @@ class TransmissionPluginLoader
TEST(DifferentialTransmissionLoaderTest, FullSpec)
{
// Parse transmission info
std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"(
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/DifferentialTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>0.5</offset>
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>-0.5</offset>
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::string urdf_to_test = ros2_control_test_assets::minimal_robot_urdf;

std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf(urdf_to_test);
ASSERT_THAT(infos[0].transmissions, SizeIs(1));
const hardware_interface::TransmissionInfo * info_ptr = nullptr;
for (const auto & info : infos)
{
for (const auto & transmission : info.transmissions)
{
if (transmission.type == "transmission_interface/DifferentialTransmission")
{
info_ptr = &transmission;
break;
}
}
if (info_ptr)
{
break;
}
}
ASSERT_TRUE(nullptr != info_ptr);

// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create(infos[0].transmissions[0].type);
loader.create(info_ptr->type);
ASSERT_TRUE(nullptr != transmission_loader);

std::shared_ptr<transmission_interface::Transmission> transmission;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load(info);
transmission = transmission_loader->load(*info_ptr);

// Validate transmission
transmission_interface::DifferentialTransmission * differential_transmission =
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,56 +56,36 @@ class TransmissionPluginLoader
TEST(FourBarLinkageTransmissionLoaderTest, FullSpec)
{
// Parse transmission info
std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + R"(
<ros2_control name="FullSpec" type="system">
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<joint name="joint2">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="position"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/FourBarLinkageTransmission</plugin>
<actuator name="joint1_motor" role="actuator1">
<mechanical_reduction>50</mechanical_reduction>
</actuator>
<actuator name="joint2_motor" role="actuator2">
<mechanical_reduction>-50</mechanical_reduction>
</actuator>
<joint name="joint1" role="joint1">
<offset>0.5</offset>
<mechanical_reduction>2.0</mechanical_reduction>
</joint>
<joint name="joint2" role="joint2">
<offset>-0.5</offset>
<mechanical_reduction>-2.0</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
</robot>
)";
std::string urdf_to_test = ros2_control_test_assets::minimal_robot_urdf;

std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf(urdf_to_test);
ASSERT_THAT(infos[0].transmissions, SizeIs(1));
const hardware_interface::TransmissionInfo * info_ptr = nullptr;
for (const auto & info : infos)
{
for (const auto & transmission : info.transmissions)
{
if (transmission.type == "transmission_interface/FourBarLinkageTransmission")
{
info_ptr = &transmission;
break;
}
}
if (info_ptr)
{
break;
}
}
ASSERT_TRUE(nullptr != info_ptr);

// Transmission loader
TransmissionPluginLoader loader;
std::shared_ptr<transmission_interface::TransmissionLoader> transmission_loader =
loader.create(infos[0].transmissions[0].type);
loader.create(info_ptr->type);
ASSERT_TRUE(nullptr != transmission_loader);

std::shared_ptr<transmission_interface::Transmission> transmission;
const hardware_interface::TransmissionInfo & info = infos[0].transmissions[0];
transmission = transmission_loader->load(info);
transmission = transmission_loader->load(*info_ptr);

// Validate transmission
transmission_interface::FourBarLinkageTransmission * four_bar_linkage_transmission =
Expand Down
57 changes: 1 addition & 56 deletions transmission_interface/test/simple_transmission_loader_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,62 +57,7 @@ TEST(SimpleTransmissionLoaderTest, FullSpec)
{
// Parse transmission info

std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) +
R"(
<ros2_control name="RRBotModularJoint1" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint1">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
<transmission name="transmission1">
<plugin>transmission_interface/SimpleTransmission</plugin>
<joint name="joint1" role="joint1">
<mechanical_reduction>325.949</mechanical_reduction>
</joint>
</transmission>
</ros2_control>
<ros2_control name="RRBotModularJoint2" type="actuator">
<hardware>
<plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
<param name="example_param_write_for_sec">1.23</param>
<param name="example_param_read_for_sec">3</param>
</hardware>
<joint name="joint2">
<command_interface name="velocity">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<state_interface name="velocity"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint1" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint1">
<state_interface name="position"/>
</joint>
</ros2_control>
<ros2_control name="RRBotModularPositionSensorJoint2" type="sensor">
<hardware>
<plugin>ros2_control_demo_hardware/PositionSensorHardware</plugin>
<param name="example_param_read_for_sec">2</param>
</hardware>
<joint name="joint2">
<state_interface name="position"/>
</joint>
</ros2_control>
</robot>
)";
std::string urdf_to_test = ros2_control_test_assets::minimal_robot_urdf;

std::vector<hardware_interface::HardwareInfo> infos =
hardware_interface::parse_control_resources_from_urdf(urdf_to_test);
Expand Down
Loading