custom_gazebo.cpp

#include "custom_sensor_plugin/custom_sensor.hpp"
#include <pluginlib/class_list_macros.hpp>
#include <gz/plugin/Register.hh>

namespace custom_sensor
{

CallbackReturn CustomSensor::on_init(const hardware_interface::HardwareInfo & info)
{
  if (info.sensors.empty()) {
    RCLCPP_ERROR(rclcpp::get_logger("CustomSensor"), "No Sensor Configuration Found");
    return CallbackReturn::ERROR;
  }
  if (info.name != "custom_sensor") {
    RCLCPP_ERROR(rclcpp::get_logger("CustomSensor"),
                "Expected 'custom_sensor' but got '%s'", info.name.c_str());
    return CallbackReturn::ERROR;
  }
  RCLCPP_INFO(rclcpp::get_logger("CustomSensor"),
              "on_init received name = '%s'", info.name.c_str());
  info_ = info;
  data_values_.resize(info_.sensors[0].state_interfaces.size(), 0.0);
  return CallbackReturn::SUCCESS;
}

CallbackReturn CustomSensor::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
{
  RCLCPP_INFO(rclcpp::get_logger("CustomSensor"), "System successfully configured!");
  return CallbackReturn::SUCCESS;
}

std::vector<hardware_interface::StateInterface> CustomSensor::export_state_interfaces()
{
  std::vector<hardware_interface::StateInterface> state_interfaces;
  const auto & sensor_info = info_.sensors[0];
  for (size_t i = 0; i < sensor_info.state_interfaces.size(); ++i) {
    state_interfaces.emplace_back(
      sensor_info.name,
      sensor_info.state_interfaces[i].name,
      &data_values_[i]);
  }
  return state_interfaces;
}

hardware_interface::return_type CustomSensor::read(
  const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/)
{
  // For testing, assign a fixed value
  for (auto & val : data_values_) {
    val = 5.0;
  }
  return hardware_interface::return_type::OK;
}

bool CustomSensor::initSim(
  rclcpp::Node::SharedPtr & model_nh,
  std::map<std::string, gz::sim::Entity> & entities,
  const hardware_interface::HardwareInfo & hardware_info,
  gz::sim::EntityComponentManager & ecm,
  int & update_rate)
{

  }

  update_rate = 0;
  return true;
}

}  // namespace custom_sensor

PLUGINLIB_EXPORT_CLASS(
  custom_sensor::CustomSensor,
  gz_ros2_control::GazeboSimSystemInterface)

IGNITION_ADD_PLUGIN(
  custom_sensor::CustomSensor,
  gz::sim::System,
  gz_ros2_control::GazeboSimSystemInterface
)