hokuyo3d.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014, ATR, Atsushi Watanabe
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <boost/bind.hpp>
31 #include <boost/chrono.hpp>
32 #include <boost/thread.hpp>
33 #include <boost/thread/lock_guard.hpp>
34 #include <boost/thread/mutex.hpp>
35 #include <boost/asio.hpp>
36 
37 #include <algorithm>
38 #include <deque>
39 #include <string>
40 
41 #include <ros/ros.h>
42 #include <sensor_msgs/PointCloud.h>
43 #include <sensor_msgs/PointCloud2.h>
45 #include <sensor_msgs/MagneticField.h>
46 #include <sensor_msgs/Imu.h>
48 
49 #include <vssp.h>
50 
52 {
53 public:
54  void cbPoint(
55  const vssp::Header &header,
56  const vssp::RangeHeader &range_header,
57  const vssp::RangeIndex &range_index,
58  const boost::shared_array<uint16_t> &index,
59  const boost::shared_array<vssp::XYZI> &points,
60  const boost::posix_time::ptime &time_read)
61  {
62  if (timestamp_base_ == ros::Time(0))
63  return;
64  // Pack scan data
65  if (enable_pc_)
66  {
67  if (cloud_.points.size() == 0)
68  {
69  // Start packing PointCloud message
70  cloud_.header.frame_id = frame_id_;
71  cloud_.header.stamp = timestamp_base_ + ros::Duration(range_header.line_head_timestamp_ms * 0.001);
72  }
73  // Pack PointCloud message
74  for (int i = 0; i < index[range_index.nspots]; i++)
75  {
76  if (points[i].r < range_min_)
77  {
78  continue;
79  }
80  geometry_msgs::Point32 point;
81  point.x = points[i].x;
82  point.y = points[i].y;
83  point.z = points[i].z;
84  cloud_.points.push_back(point);
85  cloud_.channels[0].values.push_back(points[i].i);
86  }
87  }
88  if (enable_pc2_)
89  {
90  if (cloud2_.data.size() == 0)
91  {
92  // Start packing PointCloud2 message
93  cloud2_.header.frame_id = frame_id_;
94  cloud2_.header.stamp = timestamp_base_ + ros::Duration(range_header.line_head_timestamp_ms * 0.001);
95  cloud2_.row_step = 0;
96  cloud2_.width = 0;
97  }
98  // Pack PointCloud2 message
99  cloud2_.data.resize((cloud2_.width + index[range_index.nspots]) * cloud2_.point_step);
100 
101  float *data = reinterpret_cast<float *>(&cloud2_.data[0]);
102  data += cloud2_.width * cloud2_.point_step / sizeof(float);
103  for (int i = 0; i < index[range_index.nspots]; i++)
104  {
105  if (points[i].r < range_min_)
106  {
107  continue;
108  }
109  *(data++) = points[i].x;
110  *(data++) = points[i].y;
111  *(data++) = points[i].z;
112  *(data++) = points[i].i;
113  cloud2_.width++;
114  }
115  cloud2_.row_step = cloud2_.width * cloud2_.point_step;
116  }
117  // Publish points
118  if ((cycle_ == CYCLE_FIELD && (range_header.field != field_ || range_header.frame != frame_)) ||
119  (cycle_ == CYCLE_FRAME && (range_header.frame != frame_)) || (cycle_ == CYCLE_LINE))
120  {
121  if (enable_pc_)
122  {
123  if (cloud_.header.stamp < cloud_stamp_last_ && !allow_jump_back_)
124  {
125  ROS_INFO("Dropping timestamp jump backed cloud");
126  }
127  else
128  {
130  }
131  cloud_stamp_last_ = cloud_.header.stamp;
132  cloud_.points.clear();
133  cloud_.channels[0].values.clear();
134  }
135  if (enable_pc2_)
136  {
137  cloud2_.data.resize(cloud2_.width * cloud2_.point_step);
138  if (cloud2_.header.stamp < cloud_stamp_last_ && !allow_jump_back_)
139  {
140  ROS_INFO("Dropping timestamp jump backed cloud2");
141  }
142  else
143  {
145  }
146  cloud_stamp_last_ = cloud2_.header.stamp;
147  cloud2_.data.clear();
148  }
149  if (range_header.frame != frame_)
150  ping();
151  frame_ = range_header.frame;
152  field_ = range_header.field;
153  line_ = range_header.line;
154  }
155  }
156  void cbError(
157  const vssp::Header &header,
158  const std::string &message,
159  const boost::posix_time::ptime &time_read)
160  {
161  ROS_ERROR("%s", message.c_str());
162  }
163  void cbPing(
164  const vssp::Header &header,
165  const boost::posix_time::ptime &time_read)
166  {
167  const ros::Time now = ros::Time::fromBoost(time_read);
168  const ros::Duration delay =
169  ((now - time_ping_) - ros::Duration(header.send_time_ms * 0.001 - header.received_time_ms * 0.001)) * 0.5;
170  const ros::Time base = time_ping_ + delay - ros::Duration(header.received_time_ms * 0.001);
171 
172  timestamp_base_buffer_.push_back(base);
173  if (timestamp_base_buffer_.size() > 5)
174  timestamp_base_buffer_.pop_front();
175 
176  auto sorted_timstamp_base = timestamp_base_buffer_;
177  std::sort(sorted_timstamp_base.begin(), sorted_timstamp_base.end());
178 
179  if (timestamp_base_ == ros::Time(0))
180  timestamp_base_ = sorted_timstamp_base[sorted_timstamp_base.size() / 2];
181  else
182  timestamp_base_ += (sorted_timstamp_base[sorted_timstamp_base.size() / 2] - timestamp_base_) * 0.1;
183 
184  ROS_DEBUG("timestamp_base: %lf", timestamp_base_.toSec());
185  }
186  void cbAux(
187  const vssp::Header &header,
188  const vssp::AuxHeader &aux_header,
189  const boost::shared_array<vssp::Aux> &auxs,
190  const boost::posix_time::ptime &time_read)
191  {
192  if (timestamp_base_ == ros::Time(0))
193  return;
194  ros::Time stamp = timestamp_base_ + ros::Duration(aux_header.timestamp_ms * 0.001);
195 
198  {
199  imu_.header.frame_id = imu_frame_id_;
200  imu_.header.stamp = stamp;
201  for (int i = 0; i < aux_header.data_count; i++)
202  {
203  imu_.orientation_covariance[0] = -1.0;
204  imu_.angular_velocity.x = auxs[i].ang_vel.x;
205  imu_.angular_velocity.y = auxs[i].ang_vel.y;
206  imu_.angular_velocity.z = auxs[i].ang_vel.z;
207  imu_.linear_acceleration.x = auxs[i].lin_acc.x;
208  imu_.linear_acceleration.y = auxs[i].lin_acc.y;
209  imu_.linear_acceleration.z = auxs[i].lin_acc.z;
210  if (imu_stamp_last_ > imu_.header.stamp && !allow_jump_back_)
211  {
212  ROS_INFO("Dropping timestamp jump backed imu");
213  }
214  else
215  {
217  }
218  imu_stamp_last_ = imu_.header.stamp;
219  imu_.header.stamp += ros::Duration(aux_header.data_ms * 0.001);
220  }
221  }
222  if ((aux_header.data_bitfield & vssp::AX_MASK_MAG) == vssp::AX_MASK_MAG)
223  {
224  mag_.header.frame_id = mag_frame_id_;
225  mag_.header.stamp = stamp;
226  for (int i = 0; i < aux_header.data_count; i++)
227  {
228  mag_.magnetic_field.x = auxs[i].mag.x;
229  mag_.magnetic_field.y = auxs[i].mag.y;
230  mag_.magnetic_field.z = auxs[i].mag.z;
231  if (mag_stamp_last_ > imu_.header.stamp && !allow_jump_back_)
232  {
233  ROS_INFO("Dropping timestamp jump backed mag");
234  }
235  else
236  {
238  }
239  mag_stamp_last_ = imu_.header.stamp;
240  mag_.header.stamp += ros::Duration(aux_header.data_ms * 0.001);
241  }
242  }
243  }
244  void cbConnect(bool success)
245  {
246  if (success)
247  {
248  ROS_INFO("Connection established");
249  ping();
250  if (set_auto_reset_)
256  driver_.requestData(true, true);
259  ROS_INFO("Communication started");
260  }
261  else
262  {
263  ROS_ERROR("Connection failed");
264  }
265  }
267  : pnh_("~")
268  , timestamp_base_(0)
269  , timer_(io_, boost::posix_time::milliseconds(500))
270  {
271  if (pnh_.hasParam("horizontal_interlace") || !pnh_.hasParam("interlace"))
272  {
273  pnh_.param("horizontal_interlace", horizontal_interlace_, 4);
274  }
275  else if (pnh_.hasParam("interlace"))
276  {
277  ROS_WARN("'interlace' parameter is deprecated. Use horizontal_interlace instead.");
278  pnh_.param("interlace", horizontal_interlace_, 4);
279  }
280  pnh_.param("vertical_interlace", vertical_interlace_, 1);
281  pnh_.param("ip", ip_, std::string("192.168.0.10"));
282  pnh_.param("port", port_, 10940);
283  pnh_.param("frame_id", frame_id_, std::string("hokuyo3d"));
284  pnh_.param("imu_frame_id", imu_frame_id_, frame_id_ + "_imu");
285  pnh_.param("mag_frame_id", mag_frame_id_, frame_id_ + "_mag");
286  pnh_.param("range_min", range_min_, 0.0);
287  set_auto_reset_ = pnh_.hasParam("auto_reset");
288  pnh_.param("auto_reset", auto_reset_, false);
289 
290  pnh_.param("allow_jump_back", allow_jump_back_, false);
291 
292  std::string output_cycle;
293  pnh_.param("output_cycle", output_cycle, std::string("field"));
294 
295  if (output_cycle.compare("frame") == 0)
297  else if (output_cycle.compare("field") == 0)
299  else if (output_cycle.compare("line") == 0)
300  cycle_ = CYCLE_LINE;
301  else
302  {
303  ROS_ERROR("Unknown output_cycle value %s", output_cycle.c_str());
304  ros::shutdown();
305  }
306 
307  driver_.setTimeout(2.0);
308  ROS_INFO("Connecting to %s", ip_.c_str());
309  driver_.registerCallback(boost::bind(&Hokuyo3dNode::cbPoint, this, _1, _2, _3, _4, _5, _6));
310  driver_.registerAuxCallback(boost::bind(&Hokuyo3dNode::cbAux, this, _1, _2, _3, _4));
311  driver_.registerPingCallback(boost::bind(&Hokuyo3dNode::cbPing, this, _1, _2));
312  driver_.registerErrorCallback(boost::bind(&Hokuyo3dNode::cbError, this, _1, _2, _3));
313  field_ = 0;
314  frame_ = 0;
315  line_ = 0;
316 
317  sensor_msgs::ChannelFloat32 channel;
318  channel.name = std::string("intensity");
319  cloud_.channels.push_back(channel);
320 
321  cloud2_.height = 1;
322  cloud2_.is_bigendian = false;
323  cloud2_.is_dense = false;
325  pc2_modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1,
326  sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32,
327  "intensity", 1, sensor_msgs::PointField::FLOAT32);
328 
329  pub_imu_ = pnh_.advertise<sensor_msgs::Imu>("imu", 5);
330  pub_mag_ = pnh_.advertise<sensor_msgs::MagneticField>("mag", 5);
331 
332  enable_pc_ = enable_pc2_ = false;
333  ros::SubscriberStatusCallback cb_con = boost::bind(&Hokuyo3dNode::cbSubscriber, this);
334 
335  boost::lock_guard<boost::mutex> lock(connect_mutex_);
336  pub_pc_ = pnh_.advertise<sensor_msgs::PointCloud>("hokuyo_cloud", 5, cb_con, cb_con);
337  pub_pc2_ = pnh_.advertise<sensor_msgs::PointCloud2>("hokuyo_cloud2", 5, cb_con, cb_con);
338 
339  // Start communication with the sensor
340  driver_.connect(ip_.c_str(), port_, boost::bind(&Hokuyo3dNode::cbConnect, this, _1));
341  }
343  {
344  driver_.requestAuxData(false);
345  driver_.requestData(true, false);
346  driver_.requestData(false, false);
347  driver_.poll();
348  ROS_INFO("Communication stoped");
349  }
351  {
352  boost::lock_guard<boost::mutex> lock(connect_mutex_);
353  if (pub_pc_.getNumSubscribers() > 0)
354  {
355  enable_pc_ = true;
356  ROS_DEBUG("PointCloud output enabled");
357  }
358  else
359  {
360  enable_pc_ = false;
361  ROS_DEBUG("PointCloud output disabled");
362  }
363  if (pub_pc2_.getNumSubscribers() > 0)
364  {
365  enable_pc2_ = true;
366  ROS_DEBUG("PointCloud2 output enabled");
367  }
368  else
369  {
370  enable_pc2_ = false;
371  ROS_DEBUG("PointCloud2 output disabled");
372  }
373  }
374  bool poll()
375  {
376  if (driver_.poll())
377  {
378  return true;
379  }
380  ROS_INFO("Connection closed");
381  return false;
382  }
383  void cbTimer(const boost::system::error_code& error)
384  {
385  if (error)
386  return;
387 
388  if (!ros::ok())
389  {
390  driver_.stop();
391  }
392  else
393  {
394  timer_.expires_at(
395  timer_.expires_at() +
396  boost::posix_time::milliseconds(500));
397  timer_.async_wait(boost::bind(&Hokuyo3dNode::cbTimer, this, _1));
398  }
399  }
400  void spin()
401  {
402  timer_.async_wait(boost::bind(&Hokuyo3dNode::cbTimer, this, _1));
403  boost::thread thread(
404  boost::bind(&boost::asio::io_service::run, &io_));
405 
406  ros::AsyncSpinner spinner(1);
407  spinner.start();
408  driver_.spin();
409  spinner.stop();
410  timer_.cancel();
411  ROS_INFO("Connection closed");
412  }
413  void ping()
414  {
417  }
418 
419 protected:
426  sensor_msgs::PointCloud cloud_;
427  sensor_msgs::PointCloud2 cloud2_;
428  sensor_msgs::Imu imu_;
429  sensor_msgs::MagneticField mag_;
430 
434  boost::mutex connect_mutex_;
435 
438  std::deque<ros::Time> timestamp_base_buffer_;
442 
443  boost::asio::io_service io_;
444  boost::asio::deadline_timer timer_;
445 
446  int field_;
447  int frame_;
448  int line_;
449 
451  {
455  };
457  std::string ip_;
458  int port_;
461  double range_min_;
462  std::string frame_id_;
463  std::string imu_frame_id_;
464  std::string mag_frame_id_;
467 };
468 
469 int main(int argc, char **argv)
470 {
471  ros::init(argc, argv, "hokuyo3d");
472  Hokuyo3dNode node;
473 
474  node.spin();
475 
476  return 1;
477 }
ros::Time mag_stamp_last_
Definition: hokuyo3d.cpp:440
void setAutoReset(const bool enable)
Definition: vssp.h:131
bool poll()
Definition: vssp.h:201
std::string frame_id_
Definition: hokuyo3d.cpp:462
static const uint32_t AX_MASK_MAG
Definition: vsspdefs.h:76
uint8_t data_count
Definition: vsspdefs.h:129
std::deque< ros::Time > timestamp_base_buffer_
Definition: hokuyo3d.cpp:438
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::NodeHandle pnh_
Definition: hokuyo3d.cpp:420
sensor_msgs::PointCloud2 cloud2_
Definition: hokuyo3d.cpp:427
void publish(const boost::shared_ptr< M > &message) const
void requestData(const bool intensity=1, const bool start=1)
Definition: vssp.h:180
void cbConnect(bool success)
Definition: hokuyo3d.cpp:244
void registerAuxCallback(decltype(cb_aux_) cb)
Definition: vssp.h:123
void cbSubscriber()
Definition: hokuyo3d.cpp:350
PublishCycle cycle_
Definition: hokuyo3d.cpp:456
boost::mutex connect_mutex_
Definition: hokuyo3d.cpp:434
ros::Publisher pub_pc2_
Definition: hokuyo3d.cpp:422
void requestVerticalTable(const int itl=1)
Definition: vssp.h:151
void setHorizontalInterlace(const int itl)
Definition: vssp.h:143
void setTimeout(const double to)
Definition: vssp.h:103
ros::Publisher pub_pc_
Definition: hokuyo3d.cpp:421
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void setVerticalInterlace(const int itl)
Definition: vssp.h:147
uint32_t send_time_ms
Definition: vsspdefs.h:87
static const uint32_t AX_MASK_ANGVEL
Definition: vsspdefs.h:74
void setPointCloud2Fields(int n_fields,...)
void stop()
Definition: vssp.h:213
uint16_t line
Definition: vsspdefs.h:98
int vertical_interlace_
Definition: hokuyo3d.cpp:460
#define ROS_WARN(...)
void cbPing(const vssp::Header &header, const boost::posix_time::ptime &time_read)
Definition: hokuyo3d.cpp:163
void registerCallback(decltype(cb_point_) cb)
Definition: vssp.h:119
uint32_t line_head_timestamp_ms
Definition: vsspdefs.h:92
bool set_auto_reset_
Definition: hokuyo3d.cpp:466
sensor_msgs::PointCloud cloud_
Definition: hokuyo3d.cpp:426
void registerErrorCallback(decltype(cb_error_) cb)
Definition: vssp.h:115
uint8_t field
Definition: vsspdefs.h:97
boost::asio::io_service io_
Definition: hokuyo3d.cpp:443
int horizontal_interlace_
Definition: hokuyo3d.cpp:459
void cbAux(const vssp::Header &header, const vssp::AuxHeader &aux_header, const boost::shared_array< vssp::Aux > &auxs, const boost::posix_time::ptime &time_read)
Definition: hokuyo3d.cpp:186
void requestAuxData(const bool start=1)
Definition: vssp.h:176
void cbTimer(const boost::system::error_code &error)
Definition: hokuyo3d.cpp:383
void cbError(const vssp::Header &header, const std::string &message, const boost::posix_time::ptime &time_read)
Definition: hokuyo3d.cpp:156
sensor_msgs::Imu imu_
Definition: hokuyo3d.cpp:428
uint8_t data_ms
Definition: vsspdefs.h:130
ros::Publisher pub_imu_
Definition: hokuyo3d.cpp:423
#define ROS_INFO(...)
ros::Time timestamp_base_
Definition: hokuyo3d.cpp:437
std::string mag_frame_id_
Definition: hokuyo3d.cpp:464
bool param(const std::string &param_name, T &param_val, const T &default_val) const
std::string ip_
Definition: hokuyo3d.cpp:457
uint32_t data_bitfield
Definition: vsspdefs.h:128
double range_min_
Definition: hokuyo3d.cpp:461
void registerPingCallback(decltype(cb_ping_) cb)
Definition: vssp.h:127
ROSCPP_DECL bool ok()
bool auto_reset_
Definition: hokuyo3d.cpp:465
void receivePackets()
Definition: vssp.h:191
uint8_t frame
Definition: vsspdefs.h:96
std::string imu_frame_id_
Definition: hokuyo3d.cpp:463
uint16_t nspots
Definition: vsspdefs.h:113
void requestHorizontalTable()
Definition: vssp.h:168
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void ping()
Definition: hokuyo3d.cpp:413
static const uint32_t AX_MASK_LINACC
Definition: vsspdefs.h:75
uint32_t timestamp_ms
Definition: vsspdefs.h:127
void spin()
Definition: hokuyo3d.cpp:400
boost::asio::deadline_timer timer_
Definition: hokuyo3d.cpp:444
ros::Time imu_stamp_last_
Definition: hokuyo3d.cpp:439
uint32_t getNumSubscribers() const
vssp::VsspDriver driver_
Definition: hokuyo3d.cpp:425
void spin()
Definition: vssp.h:209
void connect(const char *ip, const unsigned int port, decltype(cb_connect_) cb)
Definition: vssp.h:107
void cbPoint(const vssp::Header &header, const vssp::RangeHeader &range_header, const vssp::RangeIndex &range_index, const boost::shared_array< uint16_t > &index, const boost::shared_array< vssp::XYZI > &points, const boost::posix_time::ptime &time_read)
Definition: hokuyo3d.cpp:54
static Time now()
static Time fromBoost(const boost::posix_time::ptime &t)
ROSCPP_DECL void shutdown()
uint32_t received_time_ms
Definition: vsspdefs.h:86
ros::Time time_ping_
Definition: hokuyo3d.cpp:436
bool poll()
Definition: hokuyo3d.cpp:374
bool hasParam(const std::string &key) const
void requestPing()
Definition: vssp.h:172
bool enable_pc2_
Definition: hokuyo3d.cpp:432
ros::Publisher pub_mag_
Definition: hokuyo3d.cpp:424
#define ROS_ERROR(...)
bool allow_jump_back_
Definition: hokuyo3d.cpp:433
sensor_msgs::MagneticField mag_
Definition: hokuyo3d.cpp:429
bool enable_pc_
Definition: hokuyo3d.cpp:431
ros::Time cloud_stamp_last_
Definition: hokuyo3d.cpp:441
#define ROS_DEBUG(...)
int main(int argc, char **argv)
Definition: hokuyo3d.cpp:469


hokuyo3d
Author(s): Atsushi Watanabe
autogenerated on Sat Jun 8 2019 04:42:00