base_realsense_node.h
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2018 Intel Corporation. All Rights Reserved
3 
4 #pragma once
5 
6 #include "../include/realsense_node_factory.h"
8 
11 #include <sensor_msgs/CameraInfo.h>
13 #include <sensor_msgs/point_cloud2_iterator.h>
14 #include <sensor_msgs/Imu.h>
15 #include <nav_msgs/Odometry.h>
18 #include <condition_variable>
19 
20 #include <queue>
21 #include <mutex>
22 #include <atomic>
23 #include <thread>
24 
26 {
28  {
29  FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id) :
30  expected_frequency_(expected_frequency),
32  diagnostic_updater_(ros::NodeHandle(), ros::NodeHandle("~"), ros::this_node::getName() + "_" + name)
33  {
34  ROS_INFO("Expected frequency for %s = %.5f", name.c_str(), expected_frequency_);
37  }
38 
39  void tick()
40  {
42  }
43 
44  void update()
45  {
47  }
48 
52  };
53  typedef std::pair<image_transport::Publisher, std::shared_ptr<FrequencyDiagnostics>> ImagePublisherWithFrequencyDiagnostics;
54 
56  {
57  public:
58  TemperatureDiagnostics(std::string name, std::string serial_no);
60 
61  void update(double crnt_temperaure)
62  {
63  _crnt_temp = crnt_temperaure;
64  _updater.update();
65  }
66 
67  private:
68  double _crnt_temp;
70 
71  };
72 
74  {
75  public:
76  std::string _name;
77  std::shared_ptr<rs2::filter> _filter;
78 
79  public:
80  NamedFilter(std::string name, std::shared_ptr<rs2::filter> filter):
81  _name(name), _filter(filter)
82  {}
83  };
84 
86  {
87  public:
88  void operator()(rs2::frame f) const
89  {
90  invoke(std::move(f));
91  }
92  };
93 
95  {
96  public:
97  SyncedImuPublisher() {_is_enabled=false;};
98  SyncedImuPublisher(ros::Publisher imu_publisher, std::size_t waiting_list_size=1000);
100  void Pause(); // Pause sending messages. All messages from now on are saved in queue.
101  void Resume(); // Send all pending messages and allow sending future messages.
102  void Publish(sensor_msgs::Imu msg); //either send or hold message.
103  uint32_t getNumSubscribers() { return _publisher.getNumSubscribers();};
104  void Enable(bool is_enabled) {_is_enabled=is_enabled;};
105 
106  private:
107  void PublishPendingMessages();
108 
109  private:
110  std::mutex _mutex;
113  std::queue<sensor_msgs::Imu> _pending_messages;
114  std::size_t _waiting_list_size;
116  };
117 
119  {
120  public:
122  ros::NodeHandle& privateNodeHandle,
124  const std::string& serial_no);
125 
126  virtual void toggleSensors(bool enabled) override;
127  virtual void publishTopics() override;
128  virtual void registerDynamicReconfigCb(ros::NodeHandle& nh) override;
129  virtual ~BaseRealSenseNode();
130 
131  public:
132  enum imu_sync_method{NONE, COPY, LINEAR_INTERPOLATION};
133 
134  protected:
135  class float3
136  {
137  public:
138  float x, y, z;
139 
140  public:
141  float3& operator*=(const float& factor)
142  {
143  x*=factor;
144  y*=factor;
145  z*=factor;
146  return (*this);
147  }
148  float3& operator+=(const float3& other)
149  {
150  x+=other.x;
151  y+=other.y;
152  z+=other.z;
153  return (*this);
154  }
155  };
156 
158  std::string _base_frame_id;
159  std::string _odom_frame_id;
160  std::map<stream_index_pair, std::string> _frame_id;
161  std::map<stream_index_pair, std::string> _optical_frame_id;
162  std::map<stream_index_pair, std::string> _depth_aligned_frame_id;
163  ros::NodeHandle& _node_handle, _pnh;
165  std::vector<rs2_option> _monitor_options;
166 
167  virtual void calcAndPublishStaticTransform(const stream_index_pair& stream, const rs2::stream_profile& base_profile);
168  rs2::stream_profile getAProfile(const stream_index_pair& stream);
169  tf::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const;
170  void publish_static_tf(const ros::Time& t,
171  const float3& trans,
172  const tf::Quaternion& q,
173  const std::string& from,
174  const std::string& to);
175 
176 
177  private:
178  class CimuData
179  {
180  public:
181  CimuData() : m_time(-1) {};
182  CimuData(const stream_index_pair type, Eigen::Vector3d data, double time):
183  m_type(type),
184  m_data(data),
185  m_time(time){};
186  bool is_set() {return m_time > 0;};
187  public:
189  Eigen::Vector3d m_data;
190  double m_time;
191  };
192 
193  static std::string getNamespaceStr();
194  void getParameters();
195  void setupDevice();
196  void setupErrorCallback();
197  void setupPublishers();
198  void enable_devices();
199  void setupFilters();
200  void setupStreams();
201  bool setBaseTime(double frame_time, rs2_timestamp_domain time_domain);
202  double frameSystemTimeSec(rs2::frame frame);
203  cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image);
204  void clip_depth(rs2::depth_frame depth_frame, float clipping_dist);
205  void updateStreamCalibData(const rs2::video_stream_profile& video_profile);
206  void updateExtrinsicsCalibData(const rs2::video_stream_profile& left_video_profile, const rs2::video_stream_profile& right_video_profile);
207  void SetBaseStream();
208  void publishStaticTransforms();
209  void publishDynamicTransforms();
210  void publishIntrinsics();
211  void runFirstFrameInitialization(rs2_stream stream_type);
212  void publishPointCloud(rs2::points f, const ros::Time& t, const rs2::frameset& frameset);
213  Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics, const std::string& frame_id) const;
214 
215  IMUInfo getImuInfo(const stream_index_pair& stream_index);
216  void publishFrame(rs2::frame f, const ros::Time& t,
217  const stream_index_pair& stream,
218  std::map<stream_index_pair, cv::Mat>& images,
219  const std::map<stream_index_pair, ros::Publisher>& info_publishers,
220  const std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics>& image_publishers,
221  std::map<stream_index_pair, int>& seq,
222  std::map<stream_index_pair, sensor_msgs::CameraInfo>& camera_info,
223  const std::map<stream_index_pair, std::string>& optical_frame_id,
224  const std::map<rs2_stream, std::string>& encoding,
225  bool copy_data_from_frame = true);
226  bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile);
227 
228  void publishAlignedDepthToOthers(rs2::frameset frames, const ros::Time& t);
229  sensor_msgs::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data);
230 
231  void FillImuData_Copy(const CimuData imu_data, std::deque<sensor_msgs::Imu>& imu_msgs);
232  void ImuMessage_AddDefaultValues(sensor_msgs::Imu& imu_msg);
233  void FillImuData_LinearInterpolation(const CimuData imu_data, std::deque<sensor_msgs::Imu>& imu_msgs);
234  void imu_callback(rs2::frame frame);
235  void imu_callback_sync(rs2::frame frame, imu_sync_method sync_method=imu_sync_method::COPY);
236  void pose_callback(rs2::frame frame);
237  void multiple_message_callback(rs2::frame frame, imu_sync_method sync_method);
238  void frame_callback(rs2::frame frame);
239  void registerDynamicOption(ros::NodeHandle& nh, rs2::options sensor, std::string& module_name);
240  void readAndSetDynamicParam(ros::NodeHandle& nh1, std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure> ddynrec, const std::string option_name, const int min_val, const int max_val, rs2::sensor sensor, int* option_value);
241  void registerAutoExposureROIOptions(ros::NodeHandle& nh);
242  void set_auto_exposure_roi(const std::string option_name, rs2::sensor sensor, int new_value);
243  void set_sensor_auto_exposure_roi(rs2::sensor sensor);
244  rs2_stream rs2_string_to_stream(std::string str);
245  void startMonitoring();
246  void publish_temperature();
247  void publish_frequency_update();
248 
250  std::map<stream_index_pair, rs2::sensor> _sensors;
251  std::map<std::string, std::function<void(rs2::frame)>> _sensors_callback;
252  std::vector<std::shared_ptr<ddynamic_reconfigure::DDynamicReconfigure>> _ddynrec;
253 
254  std::string _json_file_path;
255  std::string _serial_no;
260 
261 
265 
266  std::map<stream_index_pair, rs2_intrinsics> _stream_intrinsics;
267  std::map<stream_index_pair, int> _width;
268  std::map<stream_index_pair, int> _height;
269  std::map<stream_index_pair, int> _fps;
270  std::map<rs2_stream, int> _format;
271  std::map<stream_index_pair, bool> _enable;
272  std::map<rs2_stream, std::string> _stream_name;
277  std::vector<geometry_msgs::TransformStamped> _static_tf_msgs;
278  std::shared_ptr<std::thread> _tf_t;
279 
280  std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics> _image_publishers;
281  std::map<stream_index_pair, ros::Publisher> _imu_publishers;
282  std::shared_ptr<SyncedImuPublisher> _synced_imu_publisher;
283  std::map<rs2_stream, int> _image_format;
284  std::map<stream_index_pair, ros::Publisher> _info_publisher;
285  std::map<stream_index_pair, cv::Mat> _image;
286  std::map<rs2_stream, std::string> _encoding;
287 
288  std::map<stream_index_pair, int> _seq;
289  std::map<rs2_stream, int> _unit_step_size;
290  std::map<stream_index_pair, sensor_msgs::CameraInfo> _camera_info;
291  std::atomic_bool _is_initialized_time_base;
293  std::map<stream_index_pair, std::vector<rs2::stream_profile>> _enabled_profiles;
294 
301  std::string _filters_str;
304  std::vector<NamedFilter> _filters;
305  std::vector<rs2::sensor> _dev_sensors;
306  std::map<rs2_stream, std::shared_ptr<rs2::align>> _align;
307 
308  std::map<stream_index_pair, cv::Mat> _depth_aligned_image;
309  std::map<stream_index_pair, cv::Mat> _depth_scaled_image;
310  std::map<rs2_stream, std::string> _depth_aligned_encoding;
311  std::map<stream_index_pair, sensor_msgs::CameraInfo> _depth_aligned_camera_info;
312  std::map<stream_index_pair, int> _depth_aligned_seq;
313  std::map<stream_index_pair, ros::Publisher> _depth_aligned_info_publisher;
314  std::map<stream_index_pair, ImagePublisherWithFrequencyDiagnostics> _depth_aligned_image_publishers;
315  std::map<stream_index_pair, ros::Publisher> _depth_to_other_extrinsics_publishers;
316  std::map<stream_index_pair, rs2_extrinsics> _depth_to_other_extrinsics;
317  std::map<std::string, rs2::region_of_interest> _auto_exposure_roi;
318  std::map<rs2_stream, bool> _is_first_frame;
319  std::map<rs2_stream, std::vector<std::function<void()> > > _video_functions_stack;
320 
321  typedef std::pair<rs2_option, std::shared_ptr<TemperatureDiagnostics>> OptionTemperatureDiag;
322  std::vector< OptionTemperatureDiag > _temperature_nodes;
323  std::shared_ptr<std::thread> _monitoring_t;
324  mutable std::condition_variable _cv;
325 
327  const std::string _namespace;
328 
330  std::vector< unsigned int > _valid_pc_indices;
331 
332  };//end class
333 
334 }
335 
std::map< stream_index_pair, cv::Mat > _depth_aligned_image
std::map< stream_index_pair, int > _seq
msg
std::map< rs2_stream, int > _image_format
tf2_ros::StaticTransformBroadcaster _static_tf_broadcaster
U1 type
dev
std::map< stream_index_pair, ros::Publisher > _info_publisher
tf2_ros::TransformBroadcaster _dynamic_tf_broadcaster
std::map< stream_index_pair, rs2_intrinsics > _stream_intrinsics
void setHardwareID(const std::string &hwid)
std::vector< unsigned int > _valid_pc_indices
std::string getName(void *handle)
std::pair< image_transport::Publisher, std::shared_ptr< FrequencyDiagnostics > > ImagePublisherWithFrequencyDiagnostics
void add(const std::string &name, TaskFunction f)
GLuint GLuint stream
std::map< rs2_stream, bool > _is_first_frame
std::map< stream_index_pair, ImagePublisherWithFrequencyDiagnostics > _image_publishers
GLint GLint GLint GLint GLint GLint GLint GLbitfield GLenum filter
std::queue< sensor_msgs::Imu > _pending_messages
std::pair< rs2_option, std::shared_ptr< TemperatureDiagnostics > > OptionTemperatureDiag
status
std::map< stream_index_pair, sensor_msgs::CameraInfo > _depth_aligned_camera_info
GLdouble t
std::vector< OptionTemperatureDiag > _temperature_nodes
std::map< stream_index_pair, rs2::sensor > _sensors
std::map< stream_index_pair, int > _width
GLdouble f
std::pair< rs2_stream, int > stream_index_pair
Definition: constants.h:98
sensor_msgs::PointCloud2 _msg_pointcloud
std::map< stream_index_pair, rs2_extrinsics > _depth_to_other_extrinsics
std::map< rs2_stream, std::string > _encoding
unsigned int uint32_t
std::map< stream_index_pair, bool > _enable
std::map< stream_index_pair, cv::Mat > _image
std::shared_ptr< std::thread > _tf_t
std::map< rs2_stream, std::vector< std::function< void()> > > _video_functions_stack
#define ROS_INFO(...)
diagnostic_updater::FrequencyStatus frequency_status_
std::shared_ptr< SyncedImuPublisher > _synced_imu_publisher
GLenum GLenum GLsizei const GLuint GLboolean enabled
std::map< rs2_stream, std::string > _stream_name
NamedFilter(std::string name, std::shared_ptr< rs2::filter > filter)
std::map< stream_index_pair, ImagePublisherWithFrequencyDiagnostics > _depth_aligned_image_publishers
std::map< stream_index_pair, cv::Mat > _depth_scaled_image
rs2_stream
std::function< void(stream_profile, frame_object, std::function< void()>)> frame_callback
FrequencyDiagnostics(double expected_frequency, std::string name, std::string hardware_id)
std::map< stream_index_pair, std::vector< rs2::stream_profile > > _enabled_profiles
GLdouble GLdouble GLdouble q
std::map< stream_index_pair, ros::Publisher > _imu_publishers
std::map< stream_index_pair, ros::Publisher > _depth_to_other_extrinsics_publishers
std::vector< rs2_option > _monitor_options
std::map< stream_index_pair, std::string > _frame_id
std::map< stream_index_pair, int > _height
std::map< rs2_stream, std::shared_ptr< rs2::align > > _align
std::map< stream_index_pair, sensor_msgs::CameraInfo > _camera_info
CimuData(const stream_index_pair type, Eigen::Vector3d data, double time)
std::map< rs2_stream, int > _unit_step_size
std::shared_ptr< rs2::filter > _filter
std::shared_ptr< std::thread > _monitoring_t
std::vector< geometry_msgs::TransformStamped > _static_tf_msgs
std::map< rs2_stream, std::string > _depth_aligned_encoding
std::map< stream_index_pair, int > _fps
std::map< std::string, std::function< void(rs2::frame)> > _sensors_callback
std::vector< rs2::sensor > _dev_sensors
std::vector< std::shared_ptr< ddynamic_reconfigure::DDynamicReconfigure > > _ddynrec
std::map< stream_index_pair, int > _depth_aligned_seq
std::map< std::string, rs2::region_of_interest > _auto_exposure_roi
void operator()(rs2::frame f) const
std::map< stream_index_pair, ros::Publisher > _depth_aligned_info_publisher
std::map< stream_index_pair, std::string > _optical_frame_id
std::map< stream_index_pair, std::string > _depth_aligned_frame_id
diagnostic_updater::Updater diagnostic_updater_
std::map< rs2_stream, int > _format
rs2_timestamp_domain


realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Mon Mar 1 2021 03:34:04