diff --git a/distros/rolling/patch/ros2-mujoco-ros2-control.patch b/distros/rolling/patch/ros2-mujoco-ros2-control.patch new file mode 100644 index 00000000..5bec574c --- /dev/null +++ b/distros/rolling/patch/ros2-mujoco-ros2-control.patch @@ -0,0 +1,752 @@ +diff --git a/include/mujoco_ros2_control/mujoco_system_interface.hpp b/include/mujoco_ros2_control/mujoco_system_interface.hpp +index c5f1424..8c9e5ce 100644 +--- a/include/mujoco_ros2_control/mujoco_system_interface.hpp ++++ b/include/mujoco_ros2_control/mujoco_system_interface.hpp +@@ -99,8 +99,15 @@ public: + #else + on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; + #endif ++// Jazzy+ handles own their value instead of aliasing a double*, so we build our own handles ++// and export them via on_export_*(); read()/write() sync those handles against the doubles below. ++#if ROS_DISTRO_HUMBLE + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; ++#else ++ std::vector on_export_state_interfaces() override; ++ std::vector on_export_command_interfaces() override; ++#endif + + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; +@@ -162,6 +169,47 @@ protected: + rclcpp::Logger get_logger() const; + + private: ++ /// One exported interface: the double it mirrors, and where it comes from. ++ struct InterfaceBinding ++ { ++ std::string prefix; ///< joint or sensor name ++ std::string interface_name; ///< exported interface name, e.g. hardware_interface::HW_IF_POSITION ++ double* value; ///< storage inside urdf_joint_data_ / *_sensor_data_ ++ }; ++ ++ /// Enumerates every state interface to export, in export order. ++ std::vector collect_state_interface_bindings(); ++ ++ /// Enumerates every command interface to export, in export order. ++ std::vector collect_command_interface_bindings(); ++ ++#if !ROS_DISTRO_HUMBLE ++ /// A live handle paired with the double it mirrors; built once in on_init and kept alive here. ++ template ++ struct HandleBinding ++ { ++ typename InterfaceT::SharedPtr handle; ++ double* value; ++ }; ++ ++ std::vector> state_bindings_; ++ std::vector> command_bindings_; ++ ++ /// Builds one HandleBinding per InterfaceBinding, seeding each handle from the current double. ++ template ++ static std::vector> build_bindings(const std::vector& bindings); ++ ++ /// Builds state_bindings_/command_bindings_ from collect_*_interface_bindings(). ++ void build_interface_handles(); ++ ++ /// Pushes the mirrored doubles into their exported handles; wait_for_lock controls blocking. ++ template ++ static void push_bindings_to_interfaces(const std::vector>& bindings, bool wait_for_lock); ++ ++ /// Pulls commands from the exported CommandInterface handles into their mirrored doubles. ++ void pull_commands_from_interfaces(); ++#endif ++ + /** + * @brief Loads actuator information from MuJoCo model into the SystemInterface. + * +diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp +index 36eded6..5c50a3b 100644 +--- a/src/mujoco_system_interface.cpp ++++ b/src/mujoco_system_interface.cpp +@@ -33,6 +33,8 @@ + #include + #include + #include ++#include ++#include + + #include + #include +@@ -469,6 +471,11 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf + initialize_initial_positions(get_hardware_info()); + set_initial_pose(); + ++#if !ROS_DISTRO_HUMBLE ++ // Build the owned handles now that the joint/sensor doubles have their initial values. ++ build_interface_handles(); ++#endif ++ + // Store initial state for reset_world service + simulation_->capture_initial_state(); + +@@ -509,9 +516,9 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf + return hardware_interface::CallbackReturn::SUCCESS; + } + +-std::vector MujocoSystemInterface::export_state_interfaces() ++std::vector MujocoSystemInterface::collect_state_interface_bindings() + { +- std::vector new_state_interfaces; ++ std::vector bindings; + + // Joint state interfaces + for (auto& joint : urdf_joint_data_) +@@ -523,18 +530,16 @@ std::vector MujocoSystemInterface::export_st + { + if (state_if.name == hardware_interface::HW_IF_POSITION) + { +- new_state_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, +- &joint.position_interface.state_); ++ bindings.push_back({ joint.name, hardware_interface::HW_IF_POSITION, &joint.position_interface.state_ }); + } + else if (state_if.name == hardware_interface::HW_IF_VELOCITY) + { +- new_state_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, +- &joint.velocity_interface.state_); ++ bindings.push_back({ joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_interface.state_ }); + } + else if (state_if.name == hardware_interface::HW_IF_EFFORT || + state_if.name == hardware_interface::HW_IF_TORQUE || state_if.name == hardware_interface::HW_IF_FORCE) + { +- new_state_interfaces.emplace_back(joint.name, state_if.name, &joint.effort_interface.state_); ++ bindings.push_back({ joint.name, state_if.name, &joint.effort_interface.state_ }); + } + } + } +@@ -553,27 +558,27 @@ std::vector MujocoSystemInterface::export_st + { + if (state_if.name == "force.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.force.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.force.data.x() }); + } + else if (state_if.name == "force.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.force.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.force.data.y() }); + } + else if (state_if.name == "force.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.force.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.force.data.z() }); + } + else if (state_if.name == "torque.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.torque.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.torque.data.x() }); + } + else if (state_if.name == "torque.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.torque.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.torque.data.y() }); + } + else if (state_if.name == "torque.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.torque.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.torque.data.z() }); + } + } + } +@@ -592,43 +597,43 @@ std::vector MujocoSystemInterface::export_st + { + if (state_if.name == "orientation.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.x() }); + } + else if (state_if.name == "orientation.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.y() }); + } + else if (state_if.name == "orientation.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.z() }); + } + else if (state_if.name == "orientation.w") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.w()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.w() }); + } + else if (state_if.name == "angular_velocity.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.angular_velocity.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.angular_velocity.data.x() }); + } + else if (state_if.name == "angular_velocity.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.angular_velocity.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.angular_velocity.data.y() }); + } + else if (state_if.name == "angular_velocity.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.angular_velocity.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.angular_velocity.data.z() }); + } + else if (state_if.name == "linear_acceleration.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.linear_acceleration.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.linear_acceleration.data.x() }); + } + else if (state_if.name == "linear_acceleration.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.linear_acceleration.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.linear_acceleration.data.y() }); + } + else if (state_if.name == "linear_acceleration.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.linear_acceleration.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.linear_acceleration.data.z() }); + } + // Add covariance interfaces, these aren't currently used but some controllers require them. + // TODO: Is there MuJoCo covariance data we could use? +@@ -638,7 +643,7 @@ std::vector MujocoSystemInterface::export_st + size_t idx = std::stoul(state_if.name.substr(23)); + if (idx < sensor.orientation_covariance.size()) + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation_covariance[idx]); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation_covariance[idx] }); + } + } + else if (state_if.name.find("angular_velocity_covariance") == 0) +@@ -647,7 +652,7 @@ std::vector MujocoSystemInterface::export_st + size_t idx = std::stoul(state_if.name.substr(28)); + if (idx < sensor.angular_velocity_covariance.size()) + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.angular_velocity_covariance[idx]); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.angular_velocity_covariance[idx] }); + } + } + else if (state_if.name.find("linear_acceleration_covariance") == 0) +@@ -656,7 +661,7 @@ std::vector MujocoSystemInterface::export_st + size_t idx = std::stoul(state_if.name.substr(31)); + if (idx < sensor.linear_acceleration_covariance.size()) + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.linear_acceleration_covariance[idx]); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.linear_acceleration_covariance[idx] }); + } + } + } +@@ -676,31 +681,31 @@ std::vector MujocoSystemInterface::export_st + { + if (state_if.name == "position.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.position.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.position.data.x() }); + } + else if (state_if.name == "position.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.position.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.position.data.y() }); + } + else if (state_if.name == "position.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.position.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.position.data.z() }); + } + else if (state_if.name == "orientation.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.x() }); + } + else if (state_if.name == "orientation.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.y() }); + } + else if (state_if.name == "orientation.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.z() }); + } + else if (state_if.name == "orientation.w") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.orientation.data.w()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.orientation.data.w() }); + } + } + } +@@ -719,26 +724,26 @@ std::vector MujocoSystemInterface::export_st + { + if (state_if.name == "magnetic_field.x") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.magnetic_field.data.x()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.magnetic_field.data.x() }); + } + else if (state_if.name == "magnetic_field.y") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.magnetic_field.data.y()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.magnetic_field.data.y() }); + } + else if (state_if.name == "magnetic_field.z") + { +- new_state_interfaces.emplace_back(sensor.name, state_if.name, &sensor.magnetic_field.data.z()); ++ bindings.push_back({ sensor.name, state_if.name, &sensor.magnetic_field.data.z() }); + } + } + } + } + +- return new_state_interfaces; ++ return bindings; + } + +-std::vector MujocoSystemInterface::export_command_interfaces() ++std::vector MujocoSystemInterface::collect_command_interface_bindings() + { +- std::vector new_command_interfaces; ++ std::vector bindings; + + // Joint command interfaces + for (auto& joint : urdf_joint_data_) +@@ -750,26 +755,113 @@ std::vector MujocoSystemInterface::export_ + { + if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) + { +- new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, +- &joint.position_interface.command_); ++ bindings.push_back({ joint.name, hardware_interface::HW_IF_POSITION, &joint.position_interface.command_ }); + } + else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) + { +- new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, +- &joint.velocity_interface.command_); ++ bindings.push_back({ joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_interface.command_ }); + } + else if (command_if.name == hardware_interface::HW_IF_EFFORT || + command_if.name == hardware_interface::HW_IF_TORQUE || + command_if.name == hardware_interface::HW_IF_FORCE) + { +- new_command_interfaces.emplace_back(joint.name, command_if.name, &joint.effort_interface.command_); ++ bindings.push_back({ joint.name, command_if.name, &joint.effort_interface.command_ }); + } + } + } + } + ++ return bindings; ++} ++ ++#if ROS_DISTRO_HUMBLE ++std::vector MujocoSystemInterface::export_state_interfaces() ++{ ++ std::vector new_state_interfaces; ++ for (const auto& binding : collect_state_interface_bindings()) ++ { ++ new_state_interfaces.emplace_back(binding.prefix, binding.interface_name, binding.value); ++ } ++ return new_state_interfaces; ++} ++ ++std::vector MujocoSystemInterface::export_command_interfaces() ++{ ++ std::vector new_command_interfaces; ++ for (const auto& binding : collect_command_interface_bindings()) ++ { ++ new_command_interfaces.emplace_back(binding.prefix, binding.interface_name, binding.value); ++ } + return new_command_interfaces; + } ++#else ++template ++std::vector> ++MujocoSystemInterface::build_bindings(const std::vector& bindings) ++{ ++ std::vector> handle_bindings; ++ handle_bindings.reserve(bindings.size()); ++ for (const auto& binding : bindings) ++ { ++ auto handle = std::make_shared(binding.prefix, binding.interface_name); ++ // Seed from the current value so a read/write before the next cycle sees the same value. ++ std::ignore = handle->set_value(*binding.value, true); ++ handle_bindings.push_back({ std::move(handle), binding.value }); ++ } ++ return handle_bindings; ++} ++ ++void MujocoSystemInterface::build_interface_handles() ++{ ++ state_bindings_ = build_bindings(collect_state_interface_bindings()); ++ command_bindings_ = build_bindings(collect_command_interface_bindings()); ++} ++ ++std::vector MujocoSystemInterface::on_export_state_interfaces() ++{ ++ std::vector state_interfaces; ++ state_interfaces.reserve(state_bindings_.size()); ++ for (const auto& binding : state_bindings_) ++ { ++ state_interfaces.push_back(binding.handle); ++ } ++ return state_interfaces; ++} ++ ++std::vector MujocoSystemInterface::on_export_command_interfaces() ++{ ++ std::vector command_interfaces; ++ command_interfaces.reserve(command_bindings_.size()); ++ for (const auto& binding : command_bindings_) ++ { ++ command_interfaces.push_back(binding.handle); ++ } ++ return command_interfaces; ++} ++ ++template ++void MujocoSystemInterface::push_bindings_to_interfaces(const std::vector>& bindings, ++ bool wait_for_lock) ++{ ++ for (const auto& binding : bindings) ++ { ++ std::ignore = binding.handle->set_value(*binding.value, wait_for_lock); ++ } ++} ++ ++void MujocoSystemInterface::pull_commands_from_interfaces() ++{ ++ for (const auto& binding : command_bindings_) ++ { ++ // Non-blocking: on a lock miss, leave the previous command in place. ++ double command = *binding.value; ++ if (binding.handle->get_value(command, false)) ++ { ++ *binding.value = command; ++ } ++ } ++} ++#endif + + hardware_interface::CallbackReturn MujocoSystemInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) + { +@@ -1036,12 +1128,22 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& + #endif + } + ++#if !ROS_DISTRO_HUMBLE ++ // Refresh the exported StateInterface handles now that state has been updated. ++ push_bindings_to_interfaces(state_bindings_, /*wait_for_lock=*/false); ++#endif ++ + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& /*time*/, + const rclcpp::Duration& period) + { ++#if !ROS_DISTRO_HUMBLE ++ // Refresh command_ doubles from the exported CommandInterface handles before using them below. ++ pull_commands_from_interfaces(); ++#endif ++ + // Update mimic joints + for (auto& joint : urdf_joint_data_) + { +@@ -1313,7 +1415,7 @@ bool MujocoSystemInterface::register_mujoco_actuators() + actuator_data.vel_pid = std::make_shared( + get_node(), "pid_gains.velocity." + actuator_data.joint_name, false); + actuator_data.vel_pid->initPid(); +- const auto gains = actuator_data.pos_pid->getGains(); ++ const auto gains = actuator_data.vel_pid->getGains(); + #else + actuator_data.vel_pid = std::make_shared( + get_node(), "pid_gains.velocity." + actuator_data.joint_name, "", false); +@@ -2275,6 +2377,13 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) + joint.effort_interface.command_ = 0.0; + } + ++#if !ROS_DISTRO_HUMBLE ++ // Push the reset state_/command_ doubles into their handles; block on the command push since ++ // this runs off the physics thread and a missed update here would stick until the next write. ++ push_bindings_to_interfaces(state_bindings_, /*wait_for_lock=*/false); ++ push_bindings_to_interfaces(command_bindings_, /*wait_for_lock=*/true); ++#endif ++ + // Notify plugins: they own runtime state that the world reset has invalidated. + // eq_active has already been restored to MJCF defaults. + for (auto& plugin : plugin_instances_) +diff --git a/tests/test_headless_init.cpp b/tests/test_headless_init.cpp +index ed3d231..c18f0db 100644 +--- a/tests/test_headless_init.cpp ++++ b/tests/test_headless_init.cpp +@@ -167,7 +167,11 @@ TEST_F(HeadlessInitTest, HeadlessInitialization) + ASSERT_NE(test_data, nullptr) << "Model failed to initialize within timeout"; + + // Test that we can export state interfaces without crashing ++#if ROS_DISTRO_HUMBLE + auto state_interfaces = interface_->export_state_interfaces(); ++#else ++ auto state_interfaces = interface_->on_export_state_interfaces(); ++#endif + EXPECT_GE(state_interfaces.size(), 0); + } + +diff --git a/tests/test_mujoco_system_interface.cpp b/tests/test_mujoco_system_interface.cpp +index 1aedd4f..c8deaf1 100644 +--- a/tests/test_mujoco_system_interface.cpp ++++ b/tests/test_mujoco_system_interface.cpp +@@ -215,6 +215,27 @@ protected: + #endif + } + ++ // Exports state interfaces via whichever API this distro provides, as shared pointers ++ // (std::shared_ptr, not StateInterface::SharedPtr: that typedef doesn't exist on Humble). ++ std::vector> export_state_interfaces() ++ { ++#if ROS_DISTRO_HUMBLE ++ std::vector> interfaces; ++ for (auto& state_interface : interface_->export_state_interfaces()) ++ { ++ interfaces.push_back(std::make_shared(std::move(state_interface))); ++ } ++ return interfaces; ++#else ++ std::vector> interfaces; ++ for (const auto& state_interface : interface_->on_export_state_interfaces()) ++ { ++ interfaces.push_back(std::const_pointer_cast(state_interface)); ++ } ++ return interfaces; ++#endif ++ } ++ + // Helper function to poll a condition until it returns true or the timeout expires. + bool wait_until(std::function condition, std::chrono::milliseconds timeout = std::chrono::seconds(5), + std::chrono::milliseconds poll_interval = std::chrono::milliseconds(10)) +@@ -273,12 +294,22 @@ TEST_F(MujocoSystemInterfaceTest, IntVelocityActuatorSupportsVelocityCommandInte + ASSERT_NE(actuator_id, -1); + EXPECT_EQ(model->actuator_dyntype[actuator_id], mjDYN_INTEGRATOR); + ++#if ROS_DISTRO_HUMBLE + auto command_interfaces = interface_->export_command_interfaces(); ++ auto& exported_command_interface = command_interfaces.front(); ++#else ++ auto command_interfaces = interface_->on_export_command_interfaces(); ++ auto& exported_command_interface = *command_interfaces.front(); ++#endif + ASSERT_EQ(command_interfaces.size(), 1u); +- EXPECT_EQ(command_interfaces.front().get_name(), "wheel_joint/velocity"); ++ EXPECT_EQ(exported_command_interface.get_name(), "wheel_joint/velocity"); + + constexpr double velocity_command = 2.0; +- command_interfaces.front().set_value(velocity_command); ++#if ROS_DISTRO_HUMBLE ++ exported_command_interface.set_value(velocity_command); ++#else ++ ASSERT_TRUE(exported_command_interface.set_value(velocity_command)); ++#endif + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)), + hardware_interface::return_type::OK); + +@@ -292,6 +323,59 @@ TEST_F(MujocoSystemInterfaceTest, IntVelocityActuatorSupportsVelocityCommandInte + mj_deleteModel(model); + } + ++// Pins the read()-side handle sync: without it, this would observe NaN instead of qpos/qvel. ++TEST_F(MujocoSystemInterfaceTest, JointStateInterfacesReflectSimulationAfterRead) ++{ ++ auto hardware_info = create_hardware_info(); ++ ++ hardware_interface::ComponentInfo joint_info; ++ joint_info.name = "hinge"; ++ for (const auto* interface_name : { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY }) ++ { ++ hardware_interface::InterfaceInfo state_interface; ++ state_interface.name = interface_name; ++ joint_info.state_interfaces.push_back(state_interface); ++ } ++ hardware_info.joints.push_back(joint_info); ++ ++ ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); ++ ++ // Wait for the pendulum to swing under gravity so qpos/qvel are non-zero. ++ mjModel* model = nullptr; ++ mjData* data = nullptr; ++ ASSERT_TRUE(wait_until([&]() { ++ interface_->get_model(model); ++ interface_->get_data(data); ++ return model != nullptr && data != nullptr && data->time > 0.1; ++ })) << "Simulation did not start stepping"; ++ ++ interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); ++ ++ const auto state_interfaces = export_state_interfaces(); ++ ASSERT_EQ(state_interfaces.size(), 2u); ++ ASSERT_EQ(state_interfaces[0]->get_name(), "hinge/position"); ++ ASSERT_EQ(state_interfaces[1]->get_name(), "hinge/velocity"); ++ ++#if ROS_DISTRO_HUMBLE ++ const double position = state_interfaces[0]->get_value(); ++ const double velocity = state_interfaces[1]->get_value(); ++#else ++ const double position = state_interfaces[0]->get_optional().value(); ++ const double velocity = state_interfaces[1]->get_optional().value(); ++#endif ++ ASSERT_TRUE(std::isfinite(position)) << "Exported position state interface was not synced by read()"; ++ ASSERT_TRUE(std::isfinite(velocity)) << "Exported velocity state interface was not synced by read()"; ++ ++ const int joint_id = mj_name2id(model, mjOBJ_JOINT, "hinge"); ++ ASSERT_NE(joint_id, -1); ++ const double tol = 1e-9; ++ EXPECT_NEAR(position, data->qpos[model->jnt_qposadr[joint_id]], tol); ++ EXPECT_NEAR(velocity, data->qvel[model->jnt_dofadr[joint_id]], tol); ++ ++ mj_deleteData(data); ++ mj_deleteModel(model); ++} ++ + TEST_F(MujocoSystemInterfaceTest, PoseSensorStateInterfacesRead) + { + // Register a pose sensor mapped to the framepos/framequat sensors in the MJCF model. +@@ -325,28 +409,28 @@ TEST_F(MujocoSystemInterfaceTest, PoseSensorStateInterfacesRead) + + // The pose sensor is the only component registered, so its interfaces are exported + // in registration order: position.x/y/z, orientation.x/y/z/w. +- const auto state_interfaces = interface_->export_state_interfaces(); ++ const auto state_interfaces = export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 7u); +- ASSERT_EQ(state_interfaces[0].get_name(), "pose_sensor/position.x"); +- ASSERT_EQ(state_interfaces[1].get_name(), "pose_sensor/position.y"); +- ASSERT_EQ(state_interfaces[2].get_name(), "pose_sensor/position.z"); +- ASSERT_EQ(state_interfaces[3].get_name(), "pose_sensor/orientation.x"); +- ASSERT_EQ(state_interfaces[4].get_name(), "pose_sensor/orientation.y"); +- ASSERT_EQ(state_interfaces[5].get_name(), "pose_sensor/orientation.z"); +- ASSERT_EQ(state_interfaces[6].get_name(), "pose_sensor/orientation.w"); ++ ASSERT_EQ(state_interfaces[0]->get_name(), "pose_sensor/position.x"); ++ ASSERT_EQ(state_interfaces[1]->get_name(), "pose_sensor/position.y"); ++ ASSERT_EQ(state_interfaces[2]->get_name(), "pose_sensor/position.z"); ++ ASSERT_EQ(state_interfaces[3]->get_name(), "pose_sensor/orientation.x"); ++ ASSERT_EQ(state_interfaces[4]->get_name(), "pose_sensor/orientation.y"); ++ ASSERT_EQ(state_interfaces[5]->get_name(), "pose_sensor/orientation.z"); ++ ASSERT_EQ(state_interfaces[6]->get_name(), "pose_sensor/orientation.w"); + + #if ROS_DISTRO_HUMBLE +- const double position_z = state_interfaces[2].get_value(); +- const double qx = state_interfaces[3].get_value(); +- const double qy = state_interfaces[4].get_value(); +- const double qz = state_interfaces[5].get_value(); +- const double qw = state_interfaces[6].get_value(); ++ const double position_z = state_interfaces[2]->get_value(); ++ const double qx = state_interfaces[3]->get_value(); ++ const double qy = state_interfaces[4]->get_value(); ++ const double qz = state_interfaces[5]->get_value(); ++ const double qw = state_interfaces[6]->get_value(); + #else +- const double position_z = state_interfaces[2].get_optional().value(); +- const double qx = state_interfaces[3].get_optional().value(); +- const double qy = state_interfaces[4].get_optional().value(); +- const double qz = state_interfaces[5].get_optional().value(); +- const double qw = state_interfaces[6].get_optional().value(); ++ const double position_z = state_interfaces[2]->get_optional().value(); ++ const double qx = state_interfaces[3]->get_optional().value(); ++ const double qy = state_interfaces[4]->get_optional().value(); ++ const double qz = state_interfaces[5]->get_optional().value(); ++ const double qw = state_interfaces[6]->get_optional().value(); + #endif + + // The site sits at the pendulum body origin at height 1.0, so the pose should be +@@ -383,24 +467,24 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorStateInterfacesRead) + + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + +- const auto state_interfaces = interface_->export_state_interfaces(); ++ const auto state_interfaces = export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 3u); +- ASSERT_EQ(state_interfaces[0].get_name(), "magnetometer_sensor/magnetic_field.x"); +- ASSERT_EQ(state_interfaces[1].get_name(), "magnetometer_sensor/magnetic_field.y"); +- ASSERT_EQ(state_interfaces[2].get_name(), "magnetometer_sensor/magnetic_field.z"); ++ ASSERT_EQ(state_interfaces[0]->get_name(), "magnetometer_sensor/magnetic_field.x"); ++ ASSERT_EQ(state_interfaces[1]->get_name(), "magnetometer_sensor/magnetic_field.y"); ++ ASSERT_EQ(state_interfaces[2]->get_name(), "magnetometer_sensor/magnetic_field.z"); + + const int magnetometer_id = mj_name2id(model, mjOBJ_SENSOR, "magnetometer_sensor"); + ASSERT_NE(magnetometer_id, -1); + const int magnetometer_data_index = model->sensor_adr[magnetometer_id]; + + #if ROS_DISTRO_HUMBLE +- const double magnetic_field_x = state_interfaces[0].get_value(); +- const double magnetic_field_y = state_interfaces[1].get_value(); +- const double magnetic_field_z = state_interfaces[2].get_value(); ++ const double magnetic_field_x = state_interfaces[0]->get_value(); ++ const double magnetic_field_y = state_interfaces[1]->get_value(); ++ const double magnetic_field_z = state_interfaces[2]->get_value(); + #else +- const double magnetic_field_x = state_interfaces[0].get_optional().value(); +- const double magnetic_field_y = state_interfaces[1].get_optional().value(); +- const double magnetic_field_z = state_interfaces[2].get_optional().value(); ++ const double magnetic_field_x = state_interfaces[0]->get_optional().value(); ++ const double magnetic_field_y = state_interfaces[1]->get_optional().value(); ++ const double magnetic_field_z = state_interfaces[2]->get_optional().value(); + #endif + + const double tol = 1e-9; +@@ -431,7 +515,7 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseIsApplied) + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + +- const auto state_interfaces = interface_->export_state_interfaces(); ++ const auto state_interfaces = export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + constexpr int kSamples = 2000; +@@ -442,8 +526,8 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseIsApplied) + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { +- const double raw_value = get_value(state_interfaces[axis]); +- const double noisy_value = get_value(state_interfaces[3 + axis]); ++ const double raw_value = get_value(*state_interfaces[axis]); ++ const double noisy_value = get_value(*state_interfaces[3 + axis]); + noise_samples.push_back(noisy_value - raw_value); + } + } +@@ -475,7 +559,7 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseUsesIndependentRngPerSe + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + +- const auto state_interfaces = interface_->export_state_interfaces(); ++ const auto state_interfaces = export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + bool saw_divergence = false; +@@ -484,7 +568,7 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseUsesIndependentRngPerSe + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { +- if (std::abs(get_value(state_interfaces[axis]) - get_value(state_interfaces[3 + axis])) > 1e-9) ++ if (std::abs(get_value(*state_interfaces[axis]) - get_value(*state_interfaces[3 + axis])) > 1e-9) + { + saw_divergence = true; + break; +@@ -518,7 +602,7 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseSupportsUniformDistribu + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + +- const auto state_interfaces = interface_->export_state_interfaces(); ++ const auto state_interfaces = export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + constexpr int kSamples = 2000; +@@ -529,8 +613,8 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseSupportsUniformDistribu + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { +- const double raw_value = get_value(state_interfaces[axis]); +- const double uniform_value = get_value(state_interfaces[3 + axis]); ++ const double raw_value = get_value(*state_interfaces[axis]); ++ const double uniform_value = get_value(*state_interfaces[3 + axis]); + const double sample = uniform_value - raw_value; + ASSERT_LE(sample, bound + 1e-9) << "Uniform noise must never exceed its scaled bound"; + ASSERT_GE(sample, -bound - 1e-9) << "Uniform noise must never exceed its scaled bound"; diff --git a/distros/rolling/patch/ros2-webots-ros2-control.patch b/distros/rolling/patch/ros2-webots-ros2-control.patch index 06f6149e..daac685e 100644 --- a/distros/rolling/patch/ros2-webots-ros2-control.patch +++ b/distros/rolling/patch/ros2-webots-ros2-control.patch @@ -1,8 +1,8 @@ diff --git a/include/webots_ros2_control/Ros2ControlSystem.hpp b/include/webots_ros2_control/Ros2ControlSystem.hpp -index 1a938e2..917049f 100644 +index 1a938e2..2f13c57 100644 --- a/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/include/webots_ros2_control/Ros2ControlSystem.hpp -@@ -52,11 +52,12 @@ namespace webots_ros2_control { +@@ -52,25 +52,35 @@ namespace webots_ros2_control { Ros2ControlSystem(); void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) override; @@ -17,11 +17,45 @@ index 1a938e2..917049f 100644 #endif rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & /*previous_state*/) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) override; + +- std::vector export_state_interfaces() override; +- std::vector export_command_interfaces() override; ++ // ros2_control 6.12 handles own their value instead of aliasing a double*: export our own ++ // handles, synced with the Joint doubles in read()/write(). ++ std::vector on_export_state_interfaces() override; ++ std::vector on_export_command_interfaces() override; + hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + + private: ++ template struct Binding { ++ typename InterfaceT::SharedPtr handle; ++ double *value; ++ }; ++ + webots_ros2_driver::WebotsNode *mNode; + std::vector mJoints; ++ std::vector> mStateBindings; ++ std::vector> mCommandBindings; + }; + } // namespace webots_ros2_control + diff --git a/src/Ros2ControlSystem.cpp b/src/Ros2ControlSystem.cpp -index df6c8c1..ba4b742 100644 +index df6c8c1..916a9e7 100644 --- a/src/Ros2ControlSystem.cpp +++ b/src/Ros2ControlSystem.cpp -@@ -90,18 +90,19 @@ namespace webots_ros2_control { +@@ -18,6 +18,8 @@ + #include + #include + #include ++#include ++#include + #include + + #include "hardware_interface/types/hardware_interface_type_values.hpp" +@@ -90,18 +92,19 @@ namespace webots_ros2_control { } } @@ -46,3 +80,82 @@ index df6c8c1..ba4b742 100644 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } +@@ -109,34 +112,41 @@ namespace webots_ros2_control { + } + #endif + +- std::vector Ros2ControlSystem::export_state_interfaces() { +- std::vector interfaces; ++ std::vector Ros2ControlSystem::on_export_state_interfaces() { ++ std::vector interfaces; ++ mStateBindings.clear(); ++ auto add = [&](const std::string &name, const std::string &interface, double *value) { ++ auto handle = std::make_shared(name, interface); ++ std::ignore = handle->set_value(*value, true); ++ mStateBindings.push_back({handle, value}); ++ interfaces.push_back(handle); ++ }; + for (Joint &joint : mJoints) + if (joint.sensor) { +- interfaces.emplace_back( +- hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.position))); +- interfaces.emplace_back( +- hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocity))); +- interfaces.emplace_back( +- hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_ACCELERATION, &(joint.acceleration))); ++ add(joint.name, hardware_interface::HW_IF_POSITION, &joint.position); ++ add(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity); ++ add(joint.name, hardware_interface::HW_IF_ACCELERATION, &joint.acceleration); + } +- + return interfaces; + } + +- std::vector Ros2ControlSystem::export_command_interfaces() { +- std::vector interfaces; ++ std::vector Ros2ControlSystem::on_export_command_interfaces() { ++ std::vector interfaces; ++ mCommandBindings.clear(); ++ auto add = [&](const std::string &name, const std::string &interface, double *value) { ++ auto handle = std::make_shared(name, interface); ++ std::ignore = handle->set_value(*value, true); ++ mCommandBindings.push_back({handle, value}); ++ interfaces.push_back(handle); ++ }; + for (Joint &joint : mJoints) + if (joint.motor) { + if (joint.controlPosition) +- interfaces.emplace_back( +- hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.positionCommand))); ++ add(joint.name, hardware_interface::HW_IF_POSITION, &joint.positionCommand); + if (joint.controlEffort) +- interfaces.emplace_back( +- hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_EFFORT, &(joint.effortCommand))); ++ add(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effortCommand); + if (joint.controlVelocity) +- interfaces.emplace_back( +- hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocityCommand))); ++ add(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocityCommand); + } + return interfaces; + } +@@ -169,10 +179,19 @@ namespace webots_ros2_control { + } + } + ++ // non-blocking: on a lock miss the controllers see the previous state ++ for (const auto &binding : mStateBindings) ++ std::ignore = binding.handle->set_value(*binding.value, false); + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type Ros2ControlSystem::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { ++ // non-blocking: on a lock miss the previous command stays ++ for (const auto &binding : mCommandBindings) { ++ double command = *binding.value; ++ if (binding.handle->get_value(command, false)) ++ *binding.value = command; ++ } + for (Joint &joint : mJoints) { + if (joint.motor) { + if (joint.controlPosition && !std::isnan(joint.positionCommand))