mujoco_lidar.hpp

 
#pragma once
 
#include <atomic>
#include <mutex>
#include <thread>
#include <vector>
 
#include <mujoco/mujoco.h>
 
#include <hardware_interface/hardware_info.hpp>
#include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
 
namespace mujoco_ros2_control
{
 
struct LidarData
{
  std::string name;
  std::string frame_name;
  int num_rangefinders;
  double min_angle;
  double max_angle;
  double angle_increment;
  double range_min;
  double range_max;
 
  // Maps the index of the rangefinder to the index of the MuJoCo rangefinder's data.
  // E.g. lidar-034 -> sensor_indexes[34] will contain index of that rangefinder in mj_data_->sensordata
  std::vector<int> sensor_indexes;
 
  // For message publishing
  std::string laserscan_topic;
  sensor_msgs::msg::LaserScan laser_scan_msg;
  rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scan_pub;
};
 
class MujocoLidar
{
public:
  explicit MujocoLidar(rclcpp::Node::SharedPtr node, std::recursive_mutex* sim_mutex, mjData* mujoco_data,
                       mjModel* mujoco_model, double lidar_publish_rate);
 
  void init();
 
  void close();
 
  bool register_lidar(const hardware_interface::HardwareInfo& hardware_info);
 
private:
  void update_loop();
 
  void update();
 
  rclcpp::Node::SharedPtr node_;
 
  // Ensures locked access to simulation data for rendering.
  std::recursive_mutex* sim_mutex_{ nullptr };
 
  mjData* mj_data_;
  mjModel* mj_model_;
 
  // Vector container to copy sensordata out of mj_data_
  std::vector<mjtNum> mj_lidar_data_;
 
  // LaserScan publishing rate in Hz
  double lidar_publish_rate_;
 
  // Rendering options for the cameras, currently hard coded to defaults
  mjvOption mjv_opt_;
  mjvScene mjv_scn_;
  mjrContext mjr_con_;
 
  // Containers for ladar data and ROS constructs
  std::vector<LidarData> lidar_sensors_;
 
  // Lidar processing thread
  std::thread rendering_thread_;
  std::atomic_bool publish_lidar_;
};
 
}  // namespace mujoco_ros2_control