rangefinder_lidar_plugin.hpp

 
#pragma once
 
#include <atomic>
#include <condition_variable>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
 
#include <mujoco/mujoco.h>
 
#include <rclcpp/node.hpp>
#include <rclcpp/publisher.hpp>
#include <realtime_tools/realtime_publisher.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
 
#include <mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp>
 
namespace mujoco_ros2_control_plugins
{
 
struct RangefinderLidarData
{
  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;
 
  // Raw data copied from mjData each update
  std::vector<mjtNum> raw_data;
 
  // For message publishing
  std::string laserscan_topic;
  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;
};
 
class [[deprecated("We recommend using the 3d Lidar plugin instead."
                   "See documentation for migration instructions.")]] RangefinderLidarPlugin
  : 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 std::string& lidar_name, const mjModel* model);
 
  void publish_loop();
 
  static std::pair<std::string, int> parse_lidar_name(const std::string& sensor_name);
 
  rclcpp::Node::SharedPtr node_;
  const mjModel* model_{ nullptr };
  double publish_rate_{ 5.0 };
  rclcpp::Time last_publish_time_{ 0, 0, RCL_ROS_TIME };
 
  std::vector<RangefinderLidarData> lidar_sensors_;
};
 
}  // namespace mujoco_ros2_control_plugins