From bdce61353d6bb2a459125900987c60e9a4e06dde Mon Sep 17 00:00:00 2001 From: Jens Vanhooydonck Date: Mon, 11 Sep 2023 11:06:11 +0200 Subject: [PATCH] Add Initial value for Fake System Hardware --- .../uf_robot_fake_system_hardware.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/xarm_controller/src/hardware/uf_robot_fake_system_hardware.cpp b/xarm_controller/src/hardware/uf_robot_fake_system_hardware.cpp index a80b8326..6b463e2b 100644 --- a/xarm_controller/src/hardware/uf_robot_fake_system_hardware.cpp +++ b/xarm_controller/src/hardware/uf_robot_fake_system_hardware.cpp @@ -70,6 +70,36 @@ namespace uf_robot_hardware } } + // Initialize with values from URDF + for (auto i = 0u; i < info_.joints.size(); i++) + { + const auto & joint = info_.joints[i]; + for (const auto & interface : joint.state_interfaces) + { + if (interface.name == hardware_interface::HW_IF_POSITION) + { + // Check the initial_value param is used + if (!interface.initial_value.empty()) + { + position_cmds_[i] = std::stod(interface.initial_value); + } + else + { + // Initialize the value in old way with warning message + auto it2 = joint.parameters.find("initial_" + interface.name); + if (it2 != joint.parameters.end()) + { + position_cmds_[i] = std::stod(it2->second); + RCUTILS_LOG_WARN_NAMED( + "fake_generic_system", + "The usage of initial_%s has been deprecated. Please use 'initial_value' instead.", + interface.name.c_str()); + } + } + } + } + } + RCLCPP_INFO(LOGGER, "System Sucessfully inited!"); return CallbackReturn::SUCCESS; }