mujoco_3d_lidar_plugin.hpp

 
#pragma once
 
#include <vector>
 
#include <mujoco/mujoco.h>
 
#include <geometry_msgs/msg/vector3.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp>
#include <realtime_tools/realtime_publisher.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
 
#include <mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp>
 
namespace mujoco_ros2_control_plugins
{
 
struct Lidar3dConfig
{
  std::string name;
  int sensor_id;
  int sensor_adr;
  int plugin_stateadr;
  std::string frame_name;
  std::vector<int> resolution;
  std::vector<double> azimuth_range;
  std::vector<double> elevation_range;
  std::vector<geometry_msgs::msg::Vector3> vectors;
  double range_min;
  double range_max;
  bool is_3d;
 
  // Whether or not the underlying extension is async
  bool async{ false };
 
  // For message publishing
  std::string lidar_topic;
 
  // Tracking the last update and compute time from the plugin
  mjtNum last_compute_time{ 0.0 };
  mjtNum last_published_time{ 0.0 };
 
  sensor_msgs::msg::LaserScan laser_scan_msg;
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub_raw;
  std::unique_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::LaserScan>> scan_pub;
 
  sensor_msgs::msg::PointCloud2 point_cloud_msg;
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_raw;
  std::unique_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::PointCloud2>> pointcloud_pub;
};
 
class Mujoco3dLidarPlugin : public MuJoCoROS2ControlPluginBase
{
public:
  bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override;
 
  void update_write(const mjModel* model, mjData* data) override;
 
  void cleanup() override;
 
private:
  bool register_sensor(const mjModel* model, int sensor_idx);
 
  rclcpp::Node::SharedPtr node_;
 
  // Containers for lidar data and ROS constructs
  std::vector<Lidar3dConfig> lidar_sensors_;
};
 
}  // namespace mujoco_ros2_control_plugins