hokuyo3d.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2014, ATR, Atsushi Watanabe
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the copyright holder nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <boost/bind.hpp>
00031 #include <boost/chrono.hpp>
00032 #include <boost/thread/lock_guard.hpp>
00033 #include <boost/thread/mutex.hpp>
00034 #include <string>
00035 #include <ros/ros.h>
00036 #include <sensor_msgs/PointCloud.h>
00037 #include <sensor_msgs/PointCloud2.h>
00038 #include <sensor_msgs/point_cloud2_iterator.h>
00039 #include <sensor_msgs/MagneticField.h>
00040 #include <sensor_msgs/Imu.h>
00041 #include <sensor_msgs/point_cloud_conversion.h>
00042 
00043 #include <vssp.h>
00044 
00045 class Hokuyo3dNode
00046 {
00047 public:
00048   void cbPoint(const vssp::Header &Header, const vssp::RangeHeader &RangeHeader, const vssp::RangeIndex &RangeIndex,
00049                const boost::shared_array<uint16_t> &index, const boost::shared_array<vssp::XYZI> &points,
00050                const boost::chrono::microseconds &delayRead)
00051   {
00052     if (timestamp_base_ == ros::Time(0))
00053       return;
00054     // Pack scan data
00055     if (enable_pc_)
00056     {
00057       if (cloud_.points.size() == 0)
00058       {
00059         // Start packing PointCloud message
00060         cloud_.header.frame_id = frame_id_;
00061         cloud_.header.stamp = timestamp_base_ + ros::Duration(RangeHeader.line_head_timestamp_ms * 0.001);
00062       }
00063       // Pack PointCloud message
00064       for (int i = 0; i < index[RangeIndex.nspots]; i++)
00065       {
00066         if (points[i].r < range_min_)
00067         {
00068           continue;
00069         }
00070         geometry_msgs::Point32 point;
00071         point.x = points[i].x;
00072         point.y = points[i].y;
00073         point.z = points[i].z;
00074         cloud_.points.push_back(point);
00075         cloud_.channels[0].values.push_back(points[i].i);
00076       }
00077     }
00078     if (enable_pc2_)
00079     {
00080       if (cloud2_.data.size() == 0)
00081       {
00082         // Start packing PointCloud2 message
00083         cloud2_.header.frame_id = frame_id_;
00084         cloud2_.header.stamp = timestamp_base_ + ros::Duration(RangeHeader.line_head_timestamp_ms * 0.001);
00085         cloud2_.row_step = 0;
00086         cloud2_.width = 0;
00087       }
00088       // Pack PointCloud2 message
00089       cloud2_.data.resize((cloud2_.width + index[RangeIndex.nspots]) * cloud2_.point_step);
00090 
00091       float *data = reinterpret_cast<float *>(&cloud2_.data[0]);
00092       data += cloud2_.width * cloud2_.point_step / sizeof(float);
00093       for (int i = 0; i < index[RangeIndex.nspots]; i++)
00094       {
00095         if (points[i].r < range_min_)
00096         {
00097           continue;
00098         }
00099         *(data++) = points[i].x;
00100         *(data++) = points[i].y;
00101         *(data++) = points[i].z;
00102         *(data++) = points[i].i;
00103         cloud2_.width++;
00104       }
00105       cloud2_.row_step = cloud2_.width * cloud2_.point_step;
00106     }
00107     // Publish points
00108     if ((cycle_ == CYCLE_FIELD && (RangeHeader.field != field_ || RangeHeader.frame != frame_)) ||
00109         (cycle_ == CYCLE_FRAME && (RangeHeader.frame != frame_)) || (cycle_ == CYCLE_LINE))
00110     {
00111       if (enable_pc_)
00112       {
00113         pub_pc_.publish(cloud_);
00114         cloud_.points.clear();
00115         cloud_.channels[0].values.clear();
00116       }
00117       if (enable_pc2_)
00118       {
00119         cloud2_.data.resize(cloud2_.width * cloud2_.point_step);
00120         pub_pc2_.publish(cloud2_);
00121         cloud2_.data.clear();
00122       }
00123       if (RangeHeader.frame != frame_)
00124         ping();
00125       frame_ = RangeHeader.frame;
00126       field_ = RangeHeader.field;
00127       line_ = RangeHeader.line;
00128     }
00129   };
00130   void cbError(const vssp::Header &Header, const std::string &message)
00131   {
00132     ROS_ERROR("%s", message.c_str());
00133   }
00134   void cbPing(const vssp::Header &Header, const boost::chrono::microseconds &delayRead)
00135   {
00136     ros::Time now = ros::Time::now() - ros::Duration(delayRead.count() * 0.001 * 0.001);
00137     ros::Duration delay =
00138         ((now - time_ping_) - ros::Duration(Header.send_time_ms * 0.001 - Header.received_time_ms * 0.001)) * 0.5;
00139     ros::Time base = time_ping_ + delay - ros::Duration(Header.received_time_ms * 0.001);
00140     if (timestamp_base_ == ros::Time(0))
00141       timestamp_base_ = base;
00142     else
00143       timestamp_base_ += (base - timestamp_base_) * 0.01;
00144   }
00145   void cbAux(const vssp::Header &Header, const vssp::AuxHeader &AuxHeader, const boost::shared_array<vssp::Aux> &auxs,
00146              const boost::chrono::microseconds &delayRead)
00147   {
00148     if (timestamp_base_ == ros::Time(0))
00149       return;
00150     ros::Time stamp = timestamp_base_ + ros::Duration(AuxHeader.timestamp_ms * 0.001);
00151 
00152     if ((AuxHeader.data_bitfield & (vssp::AX_MASK_ANGVEL | vssp::AX_MASK_LINACC)) ==
00153         (vssp::AX_MASK_ANGVEL | vssp::AX_MASK_LINACC))
00154     {
00155       imu_.header.frame_id = imu_frame_id_;
00156       imu_.header.stamp = stamp;
00157       for (int i = 0; i < AuxHeader.data_count; i++)
00158       {
00159         imu_.orientation_covariance[0] = -1.0;
00160         imu_.angular_velocity.x = auxs[i].ang_vel.x;
00161         imu_.angular_velocity.y = auxs[i].ang_vel.y;
00162         imu_.angular_velocity.z = auxs[i].ang_vel.z;
00163         imu_.linear_acceleration.x = auxs[i].lin_acc.x;
00164         imu_.linear_acceleration.y = auxs[i].lin_acc.y;
00165         imu_.linear_acceleration.z = auxs[i].lin_acc.z;
00166         pub_imu_.publish(imu_);
00167         imu_.header.stamp += ros::Duration(AuxHeader.data_ms * 0.001);
00168       }
00169     }
00170     if ((AuxHeader.data_bitfield & vssp::AX_MASK_MAG) == vssp::AX_MASK_MAG)
00171     {
00172       mag_.header.frame_id = mag_frame_id_;
00173       mag_.header.stamp = stamp;
00174       for (int i = 0; i < AuxHeader.data_count; i++)
00175       {
00176         mag_.magnetic_field.x = auxs[i].mag.x;
00177         mag_.magnetic_field.y = auxs[i].mag.y;
00178         mag_.magnetic_field.z = auxs[i].mag.z;
00179         pub_mag_.publish(mag_);
00180         mag_.header.stamp += ros::Duration(AuxHeader.data_ms * 0.001);
00181       }
00182     }
00183   };
00184   void cbConnect(bool success)
00185   {
00186     if (success)
00187     {
00188       ROS_INFO("Connection established");
00189       ping();
00190       driver_.setInterlace(interlace_);
00191       driver_.requestHorizontalTable();
00192       driver_.requestVerticalTable();
00193       driver_.requestData(true, true);
00194       driver_.requestAuxData();
00195       driver_.receivePackets();
00196       ROS_INFO("Communication started");
00197     }
00198     else
00199     {
00200       ROS_ERROR("Connection failed");
00201     }
00202   };
00203   Hokuyo3dNode() : pnh_("~"), timestamp_base_(0)
00204   {
00205     pnh_.param("interlace", interlace_, 4);
00206     pnh_.param("ip", ip_, std::string("192.168.0.10"));
00207     pnh_.param("port", port_, 10940);
00208     pnh_.param("frame_id", frame_id_, std::string("hokuyo3d"));
00209     pnh_.param("imu_frame_id", imu_frame_id_, frame_id_ + "_imu");
00210     pnh_.param("mag_frame_id", mag_frame_id_, frame_id_ + "_mag");
00211     pnh_.param("range_min", range_min_, 0.0);
00212 
00213     std::string output_cycle;
00214     pnh_.param("output_cycle", output_cycle, std::string("field"));
00215 
00216     if (output_cycle.compare("frame") == 0)
00217       cycle_ = CYCLE_FRAME;
00218     else if (output_cycle.compare("field") == 0)
00219       cycle_ = CYCLE_FIELD;
00220     else if (output_cycle.compare("line") == 0)
00221       cycle_ = CYCLE_LINE;
00222     else
00223     {
00224       ROS_ERROR("Unknown output_cycle value %s", output_cycle.c_str());
00225       ros::shutdown();
00226     }
00227 
00228     driver_.setTimeout(2.0);
00229     ROS_INFO("Connecting to %s", ip_.c_str());
00230     driver_.connect(ip_.c_str(), port_, boost::bind(&Hokuyo3dNode::cbConnect, this, _1));
00231     driver_.registerCallback(boost::bind(&Hokuyo3dNode::cbPoint, this, _1, _2, _3, _4, _5, _6));
00232     driver_.registerAuxCallback(boost::bind(&Hokuyo3dNode::cbAux, this, _1, _2, _3, _4));
00233     driver_.registerPingCallback(boost::bind(&Hokuyo3dNode::cbPing, this, _1, _2));
00234     driver_.registerErrorCallback(boost::bind(&Hokuyo3dNode::cbError, this, _1, _2));
00235     field_ = 0;
00236     frame_ = 0;
00237     line_ = 0;
00238 
00239     sensor_msgs::ChannelFloat32 channel;
00240     channel.name = std::string("intensity");
00241     cloud_.channels.push_back(channel);
00242 
00243     cloud2_.height = 1;
00244     cloud2_.is_bigendian = false;
00245     cloud2_.is_dense = false;
00246     sensor_msgs::PointCloud2Modifier pc2_modifier(cloud2_);
00247     pc2_modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1,
00248                                       sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32,
00249                                       "intensity", 1, sensor_msgs::PointField::FLOAT32);
00250 
00251     pub_imu_ = pnh_.advertise<sensor_msgs::Imu>("imu", 5);
00252     pub_mag_ = pnh_.advertise<sensor_msgs::MagneticField>("mag", 5);
00253 
00254     enable_pc_ = enable_pc2_ = false;
00255     ros::SubscriberStatusCallback cb_con = boost::bind(&Hokuyo3dNode::cbSubscriber, this);
00256 
00257     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00258     pub_pc_ = pnh_.advertise<sensor_msgs::PointCloud>("hokuyo_cloud", 5, cb_con, cb_con);
00259     pub_pc2_ = pnh_.advertise<sensor_msgs::PointCloud2>("hokuyo_cloud2", 5, cb_con, cb_con);
00260   };
00261   ~Hokuyo3dNode()
00262   {
00263     driver_.requestAuxData(false);
00264     driver_.requestData(true, false);
00265     driver_.requestData(false, false);
00266     driver_.poll();
00267     ROS_INFO("Communication stoped");
00268   };
00269   void cbSubscriber()
00270   {
00271     boost::lock_guard<boost::mutex> lock(connect_mutex_);
00272     if (pub_pc_.getNumSubscribers() > 0)
00273     {
00274       enable_pc_ = true;
00275       ROS_DEBUG("PointCloud output enabled");
00276     }
00277     else
00278     {
00279       enable_pc_ = false;
00280       ROS_DEBUG("PointCloud output disabled");
00281     }
00282     if (pub_pc2_.getNumSubscribers() > 0)
00283     {
00284       enable_pc2_ = true;
00285       ROS_DEBUG("PointCloud2 output enabled");
00286     }
00287     else
00288     {
00289       enable_pc2_ = false;
00290       ROS_DEBUG("PointCloud2 output disabled");
00291     }
00292   }
00293   bool poll()
00294   {
00295     if (driver_.poll())
00296     {
00297       return true;
00298     }
00299     ROS_ERROR("Connection closed");
00300     return false;
00301   };
00302   void ping()
00303   {
00304     driver_.requestPing();
00305     time_ping_ = ros::Time::now();
00306   };
00307 
00308 protected:
00309   ros::NodeHandle pnh_;
00310   ros::Publisher pub_pc_;
00311   ros::Publisher pub_pc2_;
00312   ros::Publisher pub_imu_;
00313   ros::Publisher pub_mag_;
00314   vssp::VsspDriver driver_;
00315   sensor_msgs::PointCloud cloud_;
00316   sensor_msgs::PointCloud2 cloud2_;
00317   sensor_msgs::Imu imu_;
00318   sensor_msgs::MagneticField mag_;
00319 
00320   bool enable_pc_;
00321   bool enable_pc2_;
00322   boost::mutex connect_mutex_;
00323 
00324   ros::Time time_ping_;
00325   ros::Time timestamp_base_;
00326 
00327   int field_;
00328   int frame_;
00329   int line_;
00330 
00331   enum PublishCycle
00332   {
00333     CYCLE_FIELD,
00334     CYCLE_FRAME,
00335     CYCLE_LINE
00336   };
00337   PublishCycle cycle_;
00338   std::string ip_;
00339   int port_;
00340   int interlace_;
00341   double range_min_;
00342   std::string frame_id_;
00343   std::string imu_frame_id_;
00344   std::string mag_frame_id_;
00345 };
00346 
00347 int main(int argc, char **argv)
00348 {
00349   ros::init(argc, argv, "hokuyo3d");
00350   Hokuyo3dNode node;
00351 
00352   ros::Rate wait(200);
00353 
00354   while (ros::ok())
00355   {
00356     if (!node.poll())
00357       break;
00358     ros::spinOnce();
00359     wait.sleep();
00360   }
00361 
00362   return 1;
00363 }


hokuyo3d
Author(s): Atsushi Watanabe
autogenerated on Wed Sep 27 2017 02:52:08