t265_realsense_node.cpp
Go to the documentation of this file.
1 #include "../include/t265_realsense_node.h"
2 
3 using namespace realsense2_camera;
4 
6  ros::NodeHandle& privateNodeHandle,
7  rs2::device dev,
8  const std::string& serial_no) :
9  BaseRealSenseNode(nodeHandle, privateNodeHandle, dev, serial_no),
10  _wo_snr(dev.first<rs2::wheel_odometer>()),
11  _use_odom_in(false)
12  {
15  handleWarning();
16  }
17 
19 {
20  std::string calib_odom_file;
21  _pnh.param("calib_odom_file", calib_odom_file, std::string(""));
22  if (calib_odom_file.empty())
23  {
24  ROS_INFO("No calib_odom_file. No input odometry accepted.");
25  return;
26  }
27  std::ifstream calibrationFile(calib_odom_file);
28  if (!calibrationFile)
29  {
30  ROS_FATAL_STREAM("calibration_odometry file not found. calib_odom_file = " << calib_odom_file);
31  throw std::runtime_error("calibration_odometry file not found" );
32  }
33  const std::string json_str((std::istreambuf_iterator<char>(calibrationFile)),
34  std::istreambuf_iterator<char>());
35  const std::vector<uint8_t> wo_calib(json_str.begin(), json_str.end());
36 
37  if (!_wo_snr.load_wheel_odometery_config(wo_calib))
38  {
39  ROS_FATAL_STREAM("Format error in calibration_odometry file: " << calib_odom_file);
40  throw std::runtime_error("Format error in calibration_odometry file" );
41  }
42  _use_odom_in = true;
43 }
44 
46 {
47  ROS_WARN_STREAM("toggleSensors method not implemented for T265");
48 }
49 
51 {
54 }
55 
57 {
58  rs2::log_to_callback( rs2_log_severity::RS2_LOG_SEVERITY_WARN, [&]
59  ( rs2_log_severity severity, rs2::log_message const & msg ) noexcept {
60  _T265_fault = msg.raw();
61  std::array<std::string, 2> list_of_fault{"SLAM_ERROR", "Stream transfer failed, exiting"};
62  auto it = std::find_if(begin(list_of_fault), end(list_of_fault),
63  [&](const std::string& s) {return _T265_fault.find(s) != std::string::npos; });
64  if (it != end(list_of_fault))
65  {
68  }
69  });
70 }
71 
73 {
74  if (!_use_odom_in) return;
75 
76  std::string topic_odom_in;
77  _pnh.param("topic_odom_in", topic_odom_in, DEFAULT_TOPIC_ODOM_IN);
78  ROS_INFO_STREAM("Subscribing to in_odom topic: " << topic_odom_in);
79 
81 }
82 
83 void T265RealsenseNode::odom_in_callback(const nav_msgs::Odometry::ConstPtr& msg)
84 {
85  ROS_DEBUG("Got in_odom message");
86  rs2_vector velocity {-(float)(msg->twist.twist.linear.y),
87  (float)(msg->twist.twist.linear.z),
88  -(float)(msg->twist.twist.linear.x)};
89 
90  ROS_DEBUG_STREAM("Add odom: " << velocity.x << ", " << velocity.y << ", " << velocity.z);
91  _wo_snr.send_wheel_odometry(0, 0, velocity);
92 }
93 
95 {
96  // Transform base to stream
97  tf::Quaternion quaternion_optical;
98  quaternion_optical.setRPY(M_PI / 2, 0.0, -M_PI / 2); //Pose To ROS
99  float3 zero_trans{0, 0, 0};
100 
101  ros::Time transform_ts_ = ros::Time::now();
102 
103  rs2_extrinsics ex;
104  try
105  {
106  ex = getAProfile(stream).get_extrinsics_to(base_profile);
107  }
108  catch (std::exception& e)
109  {
110  if (!strcmp(e.what(), "Requested extrinsics are not available!"))
111  {
112  ROS_WARN_STREAM(e.what() << " : using unity as default.");
113  ex = rs2_extrinsics({{1, 0, 0, 0, 1, 0, 0, 0, 1}, {0,0,0}});
114  }
115  else
116  {
117  throw e;
118  }
119  }
120 
122  Q = quaternion_optical * Q * quaternion_optical.inverse();
123  float3 trans{ex.translation[0], ex.translation[1], ex.translation[2]};
124  if (stream == POSE)
125  {
126  Q = Q.inverse();
127  publish_static_tf(transform_ts_, trans, Q, _frame_id[stream], _base_frame_id);
128  }
129  else
130  {
131  publish_static_tf(transform_ts_, trans, Q, _base_frame_id, _frame_id[stream]);
132  publish_static_tf(transform_ts_, zero_trans, quaternion_optical, _frame_id[stream], _optical_frame_id[stream]);
133 
134  // Add align_depth_to if exist:
135  if (_align_depth && _depth_aligned_frame_id.find(stream) != _depth_aligned_frame_id.end())
136  {
137  publish_static_tf(transform_ts_, trans, Q, _base_frame_id, _depth_aligned_frame_id[stream]);
138  publish_static_tf(transform_ts_, zero_trans, quaternion_optical, _depth_aligned_frame_id[stream], _optical_frame_id[stream]);
139  }
140  }
141 }
142 
144 {
145  status.summary(diagnostic_msgs::DiagnosticStatus::WARN, _T265_fault);
146 }
msg
GLuint GLuint end
GLenum GLuint GLenum severity
void log_to_callback(rs2_log_severity min_severity, S callback)
tf::Quaternion rotationMatrixToQuaternion(const float rotation[9]) const
GLdouble s
RS2_OPTION_MOTION_MODULE_TEMPERATURE
float translation[3]
void summary(unsigned char lvl, const std::string s)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void warningDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
rs2_extrinsics get_extrinsics_to(const stream_profile &to) const
T265RealsenseNode(ros::NodeHandle &nodeHandle, ros::NodeHandle &privateNodeHandle, rs2::device dev, const std::string &serial_no)
diagnostic_updater::Updater callback_updater
RS2_OPTION_ASIC_TEMPERATURE
void add(const std::string &name, TaskFunction f)
wheel_odometer
GLuint GLuint stream
e
float rotation[9]
status
not_this_one begin(...)
std::pair< rs2_stream, int > stream_index_pair
Definition: constants.h:98
const char * raw() const
#define ROS_FATAL_STREAM(args)
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
GLenum GLenum GLsizei const GLuint GLboolean enabled
GLint first
void odom_in_callback(const nav_msgs::Odometry::ConstPtr &msg)
#define ROS_WARN_STREAM(args)
const std::string DEFAULT_TOPIC_ODOM_IN
Definition: constants.h:95
#define ROS_DEBUG_STREAM(args)
Quaternion inverse() const
bool load_wheel_odometery_config(const std::vector< uint8_t > &odometry_config_buf) const
#define ROS_INFO_STREAM(args)
std::vector< rs2_option > _monitor_options
std::map< stream_index_pair, std::string > _frame_id
const stream_index_pair POSE
void calcAndPublishStaticTransform(const stream_index_pair &stream, const rs2::stream_profile &base_profile) override
void publish_static_tf(const ros::Time &t, const float3 &trans, const tf::Quaternion &q, const std::string &from, const std::string &to)
static Time now()
bool send_wheel_odometry(uint8_t wo_sensor_id, uint32_t frame_num, const rs2_vector &translational_velocity)
rs2::stream_profile getAProfile(const stream_index_pair &stream)
rs2_log_severity
std::map< stream_index_pair, std::string > _optical_frame_id
virtual void toggleSensors(bool enabled) override
std::map< stream_index_pair, std::string > _depth_aligned_frame_id
Q
#define ROS_DEBUG(...)


realsense2_camera
Author(s): Sergey Dorodnicov , Doron Hirshberg
autogenerated on Thu Apr 22 2021 02:32:07