From 68e9e3c9626aade4628c5a4c67a5e73813ba261f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Thu, 13 Dec 2018 17:03:26 +1300 Subject: [PATCH 01/14] wip organized pc with container, clean up --- velodyne_pointcloud/cfg/CloudNode.cfg | 1 + velodyne_pointcloud/cfg/TransformNode.cfg | 6 + .../include/velodyne_pointcloud/convert.h | 11 +- .../velodyne_pointcloud/datacontainerbase.h | 103 +++++++++++++++-- .../organized_cloudXYZIR.h | 29 +++++ .../velodyne_pointcloud/pointcloudXYZIR.h | 10 +- .../include/velodyne_pointcloud/rawdata.h | 14 +-- .../include/velodyne_pointcloud/transform.h | 16 +-- velodyne_pointcloud/launch/32e_points.launch | 2 + .../launch/VLP-32C_points.launch | 2 + .../launch/VLP16_points.launch | 2 + .../launch/cloud_nodelet.launch | 2 + .../launch/transform_nodelet.launch | 2 + .../src/conversions/CMakeLists.txt | 8 +- .../src/conversions/convert.cc | 40 +++++-- .../src/conversions/organized_cloudXYZIR.cc | 51 +++++++++ .../src/conversions/pointcloudXYZIR.cc | 17 ++- .../src/conversions/transform.cc | 106 +++++++----------- velodyne_pointcloud/src/lib/rawdata.cc | 14 +-- 19 files changed, 310 insertions(+), 126 deletions(-) create mode 100644 velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h create mode 100644 velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc diff --git a/velodyne_pointcloud/cfg/CloudNode.cfg b/velodyne_pointcloud/cfg/CloudNode.cfg index c4f45c783..83053a8b9 100755 --- a/velodyne_pointcloud/cfg/CloudNode.cfg +++ b/velodyne_pointcloud/cfg/CloudNode.cfg @@ -12,5 +12,6 @@ gen.add("view_direction", double_t, 0, "angle defining the center of view", 0.0, -pi, pi) gen.add("view_width", double_t, 0, "angle defining the view width", 2*pi, 0.0, 2*pi) +gen.add("organize_cloud", bool_t, 0, "organized cloud", False) exit(gen.generate(PACKAGE, "cloud_node", "CloudNode")) diff --git a/velodyne_pointcloud/cfg/TransformNode.cfg b/velodyne_pointcloud/cfg/TransformNode.cfg index e6948b9b7..68239a75c 100755 --- a/velodyne_pointcloud/cfg/TransformNode.cfg +++ b/velodyne_pointcloud/cfg/TransformNode.cfg @@ -36,4 +36,10 @@ gen.add("frame_id", "new frame of reference for point clouds", "odom") +gen.add("organize_cloud", + pgc.bool_t, + 0, + "organize cloud", + False) + exit(gen.generate(PACKAGE, "transform_node", "TransformNode")) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index ae394c6d8..13b688835 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h @@ -21,7 +21,6 @@ #include #include -#include #include #include @@ -51,9 +50,15 @@ namespace velodyne_pointcloud /// configuration parameters typedef struct { - int npackets; ///< number of packets to combine + std::string target_frame; ///< target frame + std::string fixed_frame; ///< fixed frame + bool organize_cloud; ///< enable/disable organized cloud structure + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + uint16_t num_lasers; ///< number of lasers + int npackets; ///< number of packets to combine } Config; - Config config_; + Config config_; }; } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index 9ed7b41cc..b42504684 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -1,17 +1,106 @@ - - #ifndef __DATACONTAINERBASE_H #define __DATACONTAINERBASE_H -#include +#include +#include +#include -namespace velodyne_rawdata +namespace velodyne_rawdata { - class DataContainerBase + // Shorthand typedefs for point cloud representations + typedef velodyne_pointcloud::PointXYZIR VPoint; + typedef pcl::PointCloud VPointCloud; + + class DataContainerBase { - public: + public: + DataContainerBase() : pc(new VPointCloud) {} + virtual void addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity) = 0; + virtual void newLine() = 0; + + void setParameters( + const double min_range, + const double max_range, + const std::string target_frame, + const std::string fixed_frame, + boost::shared_ptr tf_ptr = boost::shared_ptr()) + { + config_.max_range = max_range; + config_.min_range = min_range; + config_.target_frame = target_frame; + config_.fixed_frame = fixed_frame; + config_.tf_ptr = tf_ptr; + config_.transform = target_frame != pc->header.frame_id; + } + + VPointCloud::Ptr pc; + + protected: + + inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec){ + eigen_vec(0) = tf_vec[0]; + eigen_vec(1) = tf_vec[1]; + eigen_vec(2) = tf_vec[2]; + } + + bool computeTransformation(const ros::Time& time){ + tf::StampedTransform transform; + try{ + config_.tf_ptr->lookupTransform(config_.target_frame, pc->header.frame_id, time, transform); + } + catch (tf::LookupException &e){ + ROS_ERROR ("%s", e.what ()); + return false; + } + catch (tf::ExtrapolationException &e){ + ROS_ERROR ("%s", e.what ()); + return false; + } + + tf::Quaternion quaternion = transform.getRotation (); + Eigen::Quaterniond rotation ( + quaternion.w (), + quaternion.x (), + quaternion.y (), + quaternion.z () + ); + + tf::Vector3 origin = transform.getOrigin (); + Eigen::Vector3d eigen_origin; + vectorTfToEigen(origin, eigen_origin); + Eigen::Translation3d translation ( eigen_origin ); + transformation = translation * rotation; + return true; + } + + inline bool transformPoint(velodyne_rawdata::VPoint& point) + { + Eigen::Vector3d vec(point.x, point.y, point.z), out_vec; + out_vec = transformation * vec; + point.x = out_vec(0); + point.y = out_vec(1); + point.z = out_vec(2); + } + + inline bool pointInRange(float range) + { + return (range >= config_.min_range + && range <= config_.max_range); + } + + typedef struct { + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + std::string target_frame; ///< target frame to transform a point + std::string fixed_frame; ///< fixed frame used for transform + boost::shared_ptr tf_ptr; ///< transform listener + bool transform; ///< enable / disable transform points + } Config; + Config config_; + + Eigen::Affine3d transformation; + }; } #endif //__DATACONTAINERBASE_H - diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h new file mode 100644 index 000000000..3b87df6d0 --- /dev/null +++ b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h @@ -0,0 +1,29 @@ +#ifndef __ORGANIZED_CLOUDXYZIR_H +#define __ORGANIZED_CLOUDXYZIR_H + +#include + +namespace velodyne_pointcloud +{ + class OrganizedCloudXYZIR : public velodyne_rawdata::DataContainerBase + { + public: + + virtual void newLine(); + + virtual void addPoint( + const float& x, + const float& y, + const float& z, + const uint16_t& ring, + const uint16_t& azimuth, + const float& distance, + const float& intensity); + + private: + std::map organized_lasers; + uint16_t laser_cnt; + }; +} +#endif //__ORGANIZED_CLOUDXYZIR_H + diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h index cf86e268d..aef892389 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h @@ -3,18 +3,18 @@ #ifndef __POINTCLOUDXYZIR_H #define __POINTCLOUDXYZIR_H -#include +#include namespace velodyne_pointcloud { - class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase + class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase { public: - velodyne_rawdata::VPointCloud::Ptr pc; - PointcloudXYZIR() : pc(new velodyne_rawdata::VPointCloud) {} - + virtual void newLine(); + virtual void addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity); + }; } #endif //__POINTCLOUDXYZIR_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 5243703a6..27d28de10 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -30,16 +30,10 @@ #include #include #include -#include -#include namespace velodyne_rawdata { - // Shorthand typedefs for point cloud representations - typedef velodyne_pointcloud::PointXYZIR VPoint; - typedef pcl::PointCloud VPointCloud; - /** * Raw Velodyne packet constants and structures. */ @@ -163,7 +157,7 @@ namespace velodyne_rawdata double min_range; ///< minimum range to publish int min_angle; ///< minimum angle to publish int max_angle; ///< maximum angle to publish - + double tmp_min_angle; double tmp_max_angle; } Config; @@ -179,12 +173,6 @@ namespace velodyne_rawdata /** add private function to handle the VLP16 **/ void unpack_vlp16(const velodyne_msgs::VelodynePacket &pkt, DataContainerBase& data); - /** in-line test whether a point is in range */ - bool pointInRange(float range) - { - return (range >= config_.min_range - && range <= config_.max_range); - } }; } // namespace velodyne_rawdata diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h index 73e5717c5..1b1de11b8 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -61,21 +61,21 @@ namespace velodyne_pointcloud const std::string tf_prefix_; boost::shared_ptr data_; message_filters::Subscriber velodyne_scan_; - tf::MessageFilter *tf_filter_; ros::Publisher output_; - tf::TransformListener listener_; + boost::shared_ptr >tf_filter_ptr_; + boost::shared_ptr listener_ptr_; /// configuration parameters typedef struct { - std::string frame_id; ///< target frame ID + std::string target_frame; ///< target frame + std::string fixed_frame; ///< fixed frame + bool organize_cloud; ///< enable/disable organized cloud structure + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + uint16_t num_lasers; ///< number of lasers } Config; Config config_; - // Point cloud buffers for collecting points within a packet. The - // inPc_ and tfPc_ are class members only to avoid reallocation on - // every message. - PointcloudXYZIR inPc_; ///< input packet point cloud - velodyne_rawdata::VPointCloud tfPc_; ///< transformed packet point cloud }; } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch index 460687e55..e1e090b07 100644 --- a/velodyne_pointcloud/launch/32e_points.launch +++ b/velodyne_pointcloud/launch/32e_points.launch @@ -19,6 +19,7 @@ + @@ -41,6 +42,7 @@ + diff --git a/velodyne_pointcloud/launch/VLP-32C_points.launch b/velodyne_pointcloud/launch/VLP-32C_points.launch index d745c269d..71f3a41f2 100644 --- a/velodyne_pointcloud/launch/VLP-32C_points.launch +++ b/velodyne_pointcloud/launch/VLP-32C_points.launch @@ -18,6 +18,7 @@ + @@ -39,6 +40,7 @@ + diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch index 1234280f8..64376b0e9 100644 --- a/velodyne_pointcloud/launch/VLP16_points.launch +++ b/velodyne_pointcloud/launch/VLP16_points.launch @@ -19,6 +19,7 @@ + @@ -41,6 +42,7 @@ + diff --git a/velodyne_pointcloud/launch/cloud_nodelet.launch b/velodyne_pointcloud/launch/cloud_nodelet.launch index 049a89f51..e140b101f 100644 --- a/velodyne_pointcloud/launch/cloud_nodelet.launch +++ b/velodyne_pointcloud/launch/cloud_nodelet.launch @@ -6,11 +6,13 @@ + + diff --git a/velodyne_pointcloud/launch/transform_nodelet.launch b/velodyne_pointcloud/launch/transform_nodelet.launch index 465f33fc2..a18777454 100644 --- a/velodyne_pointcloud/launch/transform_nodelet.launch +++ b/velodyne_pointcloud/launch/transform_nodelet.launch @@ -7,11 +7,13 @@ + + diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt index 8712d32db..54d8240ed 100644 --- a/velodyne_pointcloud/src/conversions/CMakeLists.txt +++ b/velodyne_pointcloud/src/conversions/CMakeLists.txt @@ -1,11 +1,11 @@ -add_executable(cloud_node cloud_node.cc convert.cc pointcloudXYZIR.cc) +add_executable(cloud_node cloud_node.cc convert.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) add_dependencies(cloud_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(cloud_node velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS cloud_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -add_library(cloud_nodelet cloud_nodelet.cc convert.cc pointcloudXYZIR.cc) +add_library(cloud_nodelet cloud_nodelet.cc convert.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) add_dependencies(cloud_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(cloud_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) @@ -28,14 +28,14 @@ install(TARGETS ringcolors_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIR.cc) +add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) add_dependencies(transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(transform_node velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) install(TARGETS transform_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -add_library(transform_nodelet transform_nodelet.cc transform.cc pointcloudXYZIR.cc) +add_library(transform_nodelet transform_nodelet.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) add_dependencies(transform_nodelet ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(transform_nodelet velodyne_rawdata ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index d0b29cc0a..6bb17ce78 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -15,6 +15,8 @@ #include "velodyne_pointcloud/convert.h" +#include +#include #include namespace velodyne_pointcloud @@ -50,6 +52,9 @@ namespace velodyne_pointcloud ROS_INFO("Reconfigure Request"); data_->setParameters(config.min_range, config.max_range, config.view_direction, config.view_width); + config_.organize_cloud = config.organize_cloud; + config_.min_range = config.min_range; + config_.max_range = config.max_range; } /** @brief Callback for raw scan messages. */ @@ -58,25 +63,38 @@ namespace velodyne_pointcloud if (output_.getNumSubscribers() == 0) // no one listening? return; // avoid much work - // allocate a point cloud with same time and frame ID as raw data - PointcloudXYZIR outMsg; - // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment - outMsg.pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; - outMsg.pc->header.frame_id = scanMsg->header.frame_id; - outMsg.pc->height = 1; + velodyne_rawdata::DataContainerBase* container_ptr; + if(config_.organize_cloud) + { + container_ptr = new OrganizedCloudXYZIR(); + container_ptr->pc->width = config_.num_lasers; + } + else + { + container_ptr = new PointcloudXYZIR(); + // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment + container_ptr->pc->height = 1; + container_ptr->pc->is_dense = false; + } + + container_ptr->setParameters(config_.min_range, config_.max_range, scanMsg->header.frame_id, scanMsg->header.frame_id); + container_ptr->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; + container_ptr->pc->header.frame_id = scanMsg->header.frame_id; - outMsg.pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); + // allocate a point cloud with same time and frame ID as raw data + container_ptr->pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); // process each packet provided by the driver for (size_t i = 0; i < scanMsg->packets.size(); ++i) { - data_->unpack(scanMsg->packets[i], outMsg); + data_->unpack(scanMsg->packets[i], *container_ptr); } // publish the accumulated cloud message - ROS_DEBUG_STREAM("Publishing " << outMsg.pc->height * outMsg.pc->width - << " Velodyne points, time: " << outMsg.pc->header.stamp); - output_.publish(outMsg.pc); + ROS_DEBUG_STREAM("Publishing " << container_ptr->pc->height * container_ptr->pc->width + << " Velodyne points, time: " << container_ptr->pc->header.stamp); + output_.publish(container_ptr->pc); + delete container_ptr; } } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc new file mode 100644 index 000000000..e93001b96 --- /dev/null +++ b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc @@ -0,0 +1,51 @@ + +#include + +namespace velodyne_pointcloud +{ + void OrganizedCloudXYZIR::newLine() + { + if(config_.transform) + { + computeTransformation(ros::Time(pc->header.stamp)); + } + for (int j = 0; j < organized_lasers.size(); j++) + { + velodyne_rawdata::VPoint *point = organized_lasers[j]; + if(config_.transform) + { + transformPoint(*point); + } + pc->points.push_back(*point); + delete point; + } + // append this point to the cloud + ++pc->height; + } + + void OrganizedCloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& distance, const float& intensity) + { + velodyne_rawdata::VPoint *point_ptr = new velodyne_rawdata::VPoint; + /** The laser values are not ordered, the organized structure + * needs ordered neighbour points. The right order is defined + * by the laser_ring value. First collect each laser point + * and insert them into a map, ordered by the ring number + */ + organized_lasers[ring] = point_ptr; + point_ptr->ring = ring; + if (pointInRange(distance)) + { + point_ptr->x = x; + point_ptr->y = y; + point_ptr->z = z; + point_ptr->intensity = intensity; + } else + { + point_ptr->x = nan(""); + point_ptr->y = nan(""); + point_ptr->z = nan(""); + point_ptr->intensity = 0; + } + } +} + diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc index 3720ac626..776c26a31 100644 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc @@ -5,8 +5,18 @@ namespace velodyne_pointcloud { - void PointcloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& /*distance*/, const float& intensity) + void PointcloudXYZIR::newLine() { + if(config_.transform) + { + computeTransformation(ros::Time(pc->header.stamp)); + } + } + + void PointcloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& distance, const float& intensity) + { + if(pointInRange(distance)) + return; // convert polar coordinates to Euclidean XYZ velodyne_rawdata::VPoint point; point.ring = ring; @@ -15,6 +25,11 @@ namespace velodyne_pointcloud point.z = z; point.intensity = intensity; + if(config_.transform) + { + transformPoint(point); + } + // append this point to the cloud pc->points.push_back(point); ++pc->width; diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 47e138655..7826572ac 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -19,6 +19,8 @@ #include "velodyne_pointcloud/transform.h" +#include +#include #include namespace velodyne_pointcloud @@ -44,11 +46,10 @@ namespace velodyne_pointcloud // subscribe to VelodyneScan packets using transform filter velodyne_scan_.subscribe(node, "velodyne_packets", 10); - tf_filter_ = - new tf::MessageFilter(velodyne_scan_, - listener_, - config_.frame_id, 10); - tf_filter_->registerCallback(boost::bind(&Transform::processScan, this, _1)); + tf_filter_ptr_ = boost::shared_ptr >( + new tf::MessageFilter(velodyne_scan_, *listener_ptr_, config_.target_frame, 10)); + tf_filter_ptr_->registerCallback(boost::bind(&Transform::processScan, this, _1)); + private_nh.param("fixed_frame", config_.fixed_frame, "odom"); } void Transform::reconfigure_callback( @@ -57,8 +58,11 @@ namespace velodyne_pointcloud ROS_INFO_STREAM("Reconfigure request."); data_->setParameters(config.min_range, config.max_range, config.view_direction, config.view_width); - config_.frame_id = tf::resolve(tf_prefix_, config.frame_id); - ROS_INFO_STREAM("Target frame ID: " << config_.frame_id); + config_.target_frame = tf::resolve(tf_prefix_, config.frame_id); + ROS_INFO_STREAM("Target frame ID: " << config_.target_frame); + config_.organize_cloud = config.organize_cloud; + config_.min_range = config.min_range; + config_.max_range = config.max_range; } /** @brief Callback for raw scan messages. @@ -72,67 +76,41 @@ namespace velodyne_pointcloud if (output_.getNumSubscribers() == 0) // no one listening? return; // avoid much work - // allocate an output point cloud with same time as raw data - velodyne_rawdata::VPointCloud::Ptr outMsg(new velodyne_rawdata::VPointCloud()); - outMsg->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; - outMsg->header.frame_id = config_.frame_id; - outMsg->height = 1; + velodyne_rawdata::DataContainerBase* container_ptr; + if(config_.organize_cloud) + { + container_ptr = new OrganizedCloudXYZIR(); + container_ptr->pc->width = config_.num_lasers; + } + else + { + container_ptr = new PointcloudXYZIR(); + // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment + container_ptr->pc->height = 1; + container_ptr->pc->is_dense = false; + } + container_ptr->setParameters( + config_.min_range, config_.max_range, + config_.target_frame, config_.fixed_frame, listener_ptr_); + container_ptr->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; + container_ptr->pc->header.frame_id = scanMsg->header.frame_id; + + // allocate a point cloud with same time and frame ID as raw data + container_ptr->pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); // process each packet provided by the driver - for (size_t next = 0; next < scanMsg->packets.size(); ++next) - { - // clear input point cloud to handle this packet - inPc_.pc->points.clear(); - inPc_.pc->width = 0; - inPc_.pc->height = 1; - std_msgs::Header header; - header.stamp = scanMsg->packets[next].stamp; - header.frame_id = scanMsg->header.frame_id; - pcl_conversions::toPCL(header, inPc_.pc->header); - - // unpack the raw data - data_->unpack(scanMsg->packets[next], inPc_); - - // clear transform point cloud for this packet - tfPc_.points.clear(); // is this needed? - tfPc_.width = 0; - tfPc_.height = 1; - header.stamp = scanMsg->packets[next].stamp; - pcl_conversions::toPCL(header, tfPc_.header); - tfPc_.header.frame_id = config_.frame_id; - - // transform the packet point cloud into the target frame - try - { - ROS_DEBUG_STREAM("transforming from " << inPc_.pc->header.frame_id - << " to " << config_.frame_id); - pcl_ros::transformPointCloud(config_.frame_id, *(inPc_.pc), tfPc_, - listener_); -#if 0 // use the latest transform available, should usually work fine - pcl_ros::transformPointCloud(inPc_.pc->header.frame_id, - ros::Time(0), *(inPc_.pc), - config_.frame_id, - tfPc_, listener_); -#endif - } - catch (tf::TransformException &ex) - { - // only log tf error once every 100 times - ROS_WARN_THROTTLE(100, "%s", ex.what()); - continue; // skip this packet - } - - // append transformed packet data to end of output message - outMsg->points.insert(outMsg->points.end(), - tfPc_.points.begin(), - tfPc_.points.end()); - outMsg->width += tfPc_.points.size(); - } + for (size_t i = 0; i < scanMsg->packets.size(); ++i) + { + data_->unpack(scanMsg->packets[i], *container_ptr); + } + // did transformation of point in the container if necessary + container_ptr->pc->header.frame_id = config_.target_frame; // publish the accumulated cloud message - ROS_DEBUG_STREAM("Publishing " << outMsg->height * outMsg->width - << " Velodyne points, time: " << outMsg->header.stamp); - output_.publish(outMsg); + ROS_DEBUG_STREAM("Publishing " << container_ptr->pc->height * container_ptr->pc->width + << " Velodyne points, time: " << container_ptr->pc->header.stamp); + output_.publish(container_ptr->pc); + delete container_ptr; } } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index cc9288f90..a8db4c8da 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -197,7 +197,6 @@ inline float SQR(float val) { return val*val; } float distance = tmp.uint * calibration_.distance_resolution_m; distance += corrections.dist_correction; - if (!pointInRange(distance)) continue; /*condition added to avoid calculating points which are not in the interesting defined area (min_angle < area < max_angle)*/ @@ -304,10 +303,11 @@ inline float SQR(float val) { return val*val; } SQR(1 - static_cast(tmp.uint)/65535))); intensity = (intensity < min_intensity) ? min_intensity : intensity; intensity = (intensity > max_intensity) ? max_intensity : intensity; - + data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, raw->blocks[i].rotation, distance, intensity); } } + data.newLine(); } } @@ -374,12 +374,6 @@ inline float SQR(float val) { return val*val; } tmp.bytes[0] = raw->blocks[block].data[k]; tmp.bytes[1] = raw->blocks[block].data[k+1]; - float distance = tmp.uint * calibration_.distance_resolution_m; - distance += corrections.dist_correction; - - // skip the point if out of range - if ( !pointInRange(distance)) continue; - /** correct for the laser rotation as a function of timing during the firings **/ azimuth_corrected_f = azimuth + (azimuth_diff * ((dsr*VLP16_DSR_TOFFSET) + (firing*VLP16_FIRING_TOFFSET)) / VLP16_BLOCK_TDURATION); azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000; @@ -492,8 +486,8 @@ inline float SQR(float val) { return val*val; } data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, azimuth_corrected, distance, intensity); } } + data.newLine(); } } - } - + } } // namespace velodyne_rawdata From f01c8b0d5d58e24002cb3a02588b9532243d0001 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Thu, 7 Feb 2019 15:07:11 +0100 Subject: [PATCH 02/14] fixed container init, container is re-initialized when reconfigure is called --- .../include/velodyne_pointcloud/convert.h | 4 ++ .../velodyne_pointcloud/datacontainerbase.h | 15 +++- .../velodyne_pointcloud/pointcloudXYZIR.h | 4 +- .../include/velodyne_pointcloud/transform.h | 2 + .../src/conversions/convert.cc | 69 +++++++++++-------- .../src/conversions/pointcloudXYZIR.cc | 2 +- .../src/conversions/transform.cc | 49 ++++++++----- 7 files changed, 95 insertions(+), 50 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index 13b688835..963c5dbdd 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h @@ -48,6 +48,10 @@ namespace velodyne_pointcloud ros::Subscriber velodyne_scan_; ros::Publisher output_; + boost::shared_ptr container_ptr_; + + boost::mutex reconfigure_mtx_; + /// configuration parameters typedef struct { std::string target_frame; ///< target frame diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index b42504684..650266ea5 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -14,9 +14,15 @@ namespace velodyne_rawdata class DataContainerBase { public: - DataContainerBase() : pc(new VPointCloud) {} + DataContainerBase() : pc(new VPointCloud) { + ROS_INFO("New container"); + } - virtual void addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity) = 0; + void reset(){pc = VPointCloud::Ptr(new VPointCloud);} + virtual void addPoint( + const float& x, const float& y, const float& z, + const uint16_t& ring, const uint16_t& azimuth, + const float& distance, const float& intensity) = 0; virtual void newLine() = 0; void setParameters( @@ -24,8 +30,11 @@ namespace velodyne_rawdata const double max_range, const std::string target_frame, const std::string fixed_frame, - boost::shared_ptr tf_ptr = boost::shared_ptr()) + boost::shared_ptr tf_ptr + = boost::shared_ptr(new tf::TransformListener)) { + + ROS_INFO("Config..."); config_.max_range = max_range; config_.min_range = min_range; config_.target_frame = target_frame; diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h index aef892389..058cb2a2d 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h @@ -13,7 +13,9 @@ namespace velodyne_pointcloud virtual void newLine(); - virtual void addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity); + virtual void addPoint(const float& x, const float& y, const float& z, + const uint16_t& ring, const uint16_t& azimuth, + const float& distance, const float& intensity); }; } diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h index 1b1de11b8..0ac5d7134 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -76,6 +76,8 @@ namespace velodyne_pointcloud } Config; Config config_; + boost::shared_ptr container_ptr; + }; } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index 6bb17ce78..176396037 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -27,6 +27,20 @@ namespace velodyne_pointcloud { data_->setup(private_nh); + if(config_.organize_cloud) + { + container_ptr_ = boost::shared_ptr(new OrganizedCloudXYZIR); + container_ptr_->pc->width = config_.num_lasers; + container_ptr_->pc->is_dense = false; + } + else + { + container_ptr_ = boost::shared_ptr(new PointcloudXYZIR); + container_ptr_->pc->height = 1; + container_ptr_->pc->is_dense = true; + } + + container_ptr_->setParameters(config_.min_range, config_.max_range, config_.target_frame, config_.fixed_frame); // advertise output point cloud (before subscribing to input data) output_ = @@ -49,52 +63,53 @@ namespace velodyne_pointcloud void Convert::callback(velodyne_pointcloud::CloudNodeConfig &config, uint32_t level) { - ROS_INFO("Reconfigure Request"); - data_->setParameters(config.min_range, config.max_range, config.view_direction, - config.view_width); + ROS_INFO("Reconfigure Request"); + data_->setParameters(config.min_range, config.max_range, config.view_direction, + config.view_width); config_.organize_cloud = config.organize_cloud; config_.min_range = config.min_range; config_.max_range = config.max_range; - } - /** @brief Callback for raw scan messages. */ - void Convert::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg) - { - if (output_.getNumSubscribers() == 0) // no one listening? - return; // avoid much work - velodyne_rawdata::DataContainerBase* container_ptr; - if(config_.organize_cloud) + if(config_.organize_cloud)// TODO only on change { - container_ptr = new OrganizedCloudXYZIR(); - container_ptr->pc->width = config_.num_lasers; + boost::lock_guard guard(reconfigure_mtx_); + container_ptr_ = boost::shared_ptr(new OrganizedCloudXYZIR); + container_ptr_->pc->width = config_.num_lasers; + container_ptr_->pc->is_dense = false; } else { - container_ptr = new PointcloudXYZIR(); - // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment - container_ptr->pc->height = 1; - container_ptr->pc->is_dense = false; + boost::lock_guard guard(reconfigure_mtx_); + container_ptr_ = boost::shared_ptr(new PointcloudXYZIR); + container_ptr_->pc->height = 1; + container_ptr_->pc->is_dense = true; } - container_ptr->setParameters(config_.min_range, config_.max_range, scanMsg->header.frame_id, scanMsg->header.frame_id); - container_ptr->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; - container_ptr->pc->header.frame_id = scanMsg->header.frame_id; + container_ptr_->setParameters(config_.min_range, config_.max_range, config_.target_frame, config_.fixed_frame); - // allocate a point cloud with same time and frame ID as raw data - container_ptr->pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); + } + /** @brief Callback for raw scan messages. */ + void Convert::processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg) + { + if (output_.getNumSubscribers() == 0) // no one listening? + return; // avoid much work + + // allocate a point cloud with same time and frame ID as raw data + container_ptr_->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; + container_ptr_->pc->header.frame_id = scanMsg->header.frame_id; + container_ptr_->pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); // process each packet provided by the driver for (size_t i = 0; i < scanMsg->packets.size(); ++i) { - data_->unpack(scanMsg->packets[i], *container_ptr); + data_->unpack(scanMsg->packets[i], *container_ptr_); } // publish the accumulated cloud message - ROS_DEBUG_STREAM("Publishing " << container_ptr->pc->height * container_ptr->pc->width - << " Velodyne points, time: " << container_ptr->pc->header.stamp); - output_.publish(container_ptr->pc); - delete container_ptr; + ROS_DEBUG_STREAM("Publishing " << container_ptr_->pc->height * container_ptr_->pc->width + << " Velodyne points, time: " << container_ptr_->pc->header.stamp); + output_.publish(container_ptr_->pc); } } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc index 776c26a31..c3a3e09da 100644 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc @@ -9,7 +9,7 @@ namespace velodyne_pointcloud { if(config_.transform) { - computeTransformation(ros::Time(pc->header.stamp)); + computeTransformation(pcl_conversions::fromPCL(pc->header.stamp)); } } diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 7826572ac..9ba83958e 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -50,6 +50,19 @@ namespace velodyne_pointcloud new tf::MessageFilter(velodyne_scan_, *listener_ptr_, config_.target_frame, 10)); tf_filter_ptr_->registerCallback(boost::bind(&Transform::processScan, this, _1)); private_nh.param("fixed_frame", config_.fixed_frame, "odom"); + + if(config_.organize_cloud) + { + container_ptr = boost::shared_ptr(new OrganizedCloudXYZIR); + container_ptr->pc->width = config_.num_lasers; + container_ptr->pc->is_dense = false; + } + else + { + container_ptr = boost::shared_ptr(new PointcloudXYZIR); + container_ptr->pc->height = 1; + container_ptr->pc->is_dense = true; + } } void Transform::reconfigure_callback( @@ -63,6 +76,22 @@ namespace velodyne_pointcloud config_.organize_cloud = config.organize_cloud; config_.min_range = config.min_range; config_.max_range = config.max_range; + + if(config_.organize_cloud) + { + container_ptr = boost::shared_ptr(new OrganizedCloudXYZIR); + container_ptr->pc->width = config_.num_lasers; + container_ptr->pc->is_dense = false; + } + else + { + container_ptr = boost::shared_ptr(new PointcloudXYZIR); + container_ptr->pc->height = 1; + container_ptr->pc->is_dense = true; + } + container_ptr->setParameters( + config_.min_range, config_.max_range, + config_.target_frame, config_.fixed_frame, listener_ptr_); } /** @brief Callback for raw scan messages. @@ -76,23 +105,8 @@ namespace velodyne_pointcloud if (output_.getNumSubscribers() == 0) // no one listening? return; // avoid much work - velodyne_rawdata::DataContainerBase* container_ptr; - if(config_.organize_cloud) - { - container_ptr = new OrganizedCloudXYZIR(); - container_ptr->pc->width = config_.num_lasers; - } - else - { - container_ptr = new PointcloudXYZIR(); - // outMsg's header is a pcl::PCLHeader, convert it before stamp assignment - container_ptr->pc->height = 1; - container_ptr->pc->is_dense = false; - } - container_ptr->setParameters( - config_.min_range, config_.max_range, - config_.target_frame, config_.fixed_frame, listener_ptr_); - container_ptr->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; + container_ptr->reset(); + container_ptr->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header.stamp); container_ptr->pc->header.frame_id = scanMsg->header.frame_id; // allocate a point cloud with same time and frame ID as raw data @@ -110,7 +124,6 @@ namespace velodyne_pointcloud ROS_DEBUG_STREAM("Publishing " << container_ptr->pc->height * container_ptr->pc->width << " Velodyne points, time: " << container_ptr->pc->header.stamp); output_.publish(container_ptr->pc); - delete container_ptr; } } // namespace velodyne_pointcloud From 899ad3e342bed13793476064933d218e322314c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Fri, 8 Feb 2019 15:15:43 +0100 Subject: [PATCH 03/14] fixed in range min max, and cleaned up the initialization --- velodyne_driver/launch/nodelet_manager.launch | 2 +- .../include/velodyne_pointcloud/convert.h | 1 + .../velodyne_pointcloud/datacontainerbase.h | 93 +++++++++++++------ .../organized_cloudXYZIR.h | 6 +- .../velodyne_pointcloud/pointcloudXYZIR.h | 5 + .../include/velodyne_pointcloud/rawdata.h | 2 +- .../include/velodyne_pointcloud/transform.h | 2 + .../src/conversions/convert.cc | 74 +++++++++------ .../src/conversions/organized_cloudXYZIR.cc | 15 ++- .../src/conversions/pointcloudXYZIR.cc | 14 ++- .../src/conversions/transform.cc | 78 +++++++++------- velodyne_pointcloud/src/lib/rawdata.cc | 12 +-- 12 files changed, 204 insertions(+), 100 deletions(-) diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch index 44eb60c4a..64e1886fe 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -16,7 +16,7 @@ - + #include #include +#include namespace velodyne_rawdata { @@ -14,33 +15,78 @@ namespace velodyne_rawdata class DataContainerBase { public: - DataContainerBase() : pc(new VPointCloud) { - ROS_INFO("New container"); + DataContainerBase( + const double max_range, const double min_range, + const std::string& target_frame, const std::string& fixed_frame, + const unsigned int init_width, const unsigned int init_height, + const bool is_dense, const unsigned int scans_per_packet) + : config_(max_range, min_range, target_frame, fixed_frame, + init_width, init_height, is_dense, scans_per_packet), pc(new VPointCloud) { + + if(config_.transform) + { + tf_ptr = boost::shared_ptr(new tf::TransformListener); + } + + } + + struct Config { + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + std::string target_frame; ///< target frame to transform a point + std::string fixed_frame; ///< fixed frame used for transform + unsigned int init_width; + unsigned int init_height; + bool is_dense; + unsigned int scans_per_packet; + bool transform; ///< enable / disable transform points + + Config( + double max_range, double min_range, + std::string target_frame, std::string fixed_frame, + unsigned int init_width, unsigned int init_height, + bool is_dense, unsigned int scans_per_packet) + : max_range(max_range), min_range(min_range), + target_frame(target_frame), fixed_frame(fixed_frame), + transform(fixed_frame!=target_frame), + init_width(init_width), init_height(init_height), + is_dense(is_dense), scans_per_packet(scans_per_packet) + { + ROS_INFO_STREAM("Initialized container with " + << "min_range: "<< min_range << ", max_range: " << max_range + << ", target_frame: " << target_frame << ", fixed_frame: " << fixed_frame + << ", init_with: " << init_width << ", init_height: " << init_height + << ", is_dense: " << is_dense << ", scans_per_packet: " << scans_per_packet); + } + }; + + void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ + pc = VPointCloud::Ptr(new VPointCloud); + pc->header.stamp = pcl_conversions::toPCL(scan_msg->header.stamp); + pc->header.frame_id = scan_msg->header.frame_id; + pc->points.reserve(scan_msg->packets.size() * config_.scans_per_packet); + pc->width = config_.init_width; + pc->height = config_.init_height; + pc->is_dense = config_.is_dense; } - void reset(){pc = VPointCloud::Ptr(new VPointCloud);} virtual void addPoint( const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& azimuth, const float& distance, const float& intensity) = 0; virtual void newLine() = 0; - void setParameters( - const double min_range, - const double max_range, - const std::string target_frame, - const std::string fixed_frame, - boost::shared_ptr tf_ptr - = boost::shared_ptr(new tf::TransformListener)) - { - - ROS_INFO("Config..."); + void configure(const double max_range, const double min_range, + const std::string fixed_frame, const std::string target_frame) { config_.max_range = max_range; config_.min_range = min_range; - config_.target_frame = target_frame; config_.fixed_frame = fixed_frame; - config_.tf_ptr = tf_ptr; - config_.transform = target_frame != pc->header.frame_id; + config_.target_frame = target_frame; + config_.transform = fixed_frame != target_frame; + if(config_.transform) + { + tf_ptr = boost::shared_ptr(new tf::TransformListener); + } } VPointCloud::Ptr pc; @@ -56,7 +102,7 @@ namespace velodyne_rawdata bool computeTransformation(const ros::Time& time){ tf::StampedTransform transform; try{ - config_.tf_ptr->lookupTransform(config_.target_frame, pc->header.frame_id, time, transform); + tf_ptr->lookupTransform(config_.target_frame, pc->header.frame_id, time, transform); } catch (tf::LookupException &e){ ROS_ERROR ("%s", e.what ()); @@ -98,18 +144,11 @@ namespace velodyne_rawdata && range <= config_.max_range); } - typedef struct { - double max_range; ///< maximum range to publish - double min_range; ///< minimum range to publish - std::string target_frame; ///< target frame to transform a point - std::string fixed_frame; ///< fixed frame used for transform - boost::shared_ptr tf_ptr; ///< transform listener - bool transform; ///< enable / disable transform points - } Config; Config config_; - Eigen::Affine3d transformation; + boost::shared_ptr tf_ptr; ///< transform listener - }; + Eigen::Affine3d transformation; +}; } #endif //__DATACONTAINERBASE_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h index 3b87df6d0..2c2556ccf 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h @@ -9,6 +9,11 @@ namespace velodyne_pointcloud { public: + OrganizedCloudXYZIR( + const double max_range, const double min_range, + const std::string& target_frame, const std::string& fixed_frame, + const unsigned int num_lasers, const unsigned int scans_per_block); + virtual void newLine(); virtual void addPoint( @@ -22,7 +27,6 @@ namespace velodyne_pointcloud private: std::map organized_lasers; - uint16_t laser_cnt; }; } #endif //__ORGANIZED_CLOUDXYZIR_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h index 058cb2a2d..3572b5384 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h @@ -11,6 +11,11 @@ namespace velodyne_pointcloud { public: + PointcloudXYZIR( + const double max_range, const double min_range, + const std::string& target_frame, const std::string& fixed_frame, + const unsigned int scans_per_block); + virtual void newLine(); virtual void addPoint(const float& x, const float& y, const float& z, diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 27d28de10..a772d47f2 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -126,7 +126,7 @@ namespace velodyne_rawdata * @returns 0 if successful; * errno value for failure */ - int setup(ros::NodeHandle private_nh); + boost::optional setup(ros::NodeHandle private_nh); /** \brief Set up for data processing offline. * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle. diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h index 0ac5d7134..d464f25d2 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -78,6 +78,8 @@ namespace velodyne_pointcloud boost::shared_ptr container_ptr; + boost::mutex reconfigure_mtx_; + }; } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index 176396037..0644a4957 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -23,24 +23,37 @@ namespace velodyne_pointcloud { /** @brief Constructor. */ Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh): - data_(new velodyne_rawdata::RawData()) + data_(new velodyne_rawdata::RawData()), first_rcfg_call(true) { - data_->setup(private_nh); + + boost::optional calibration = data_->setup(private_nh); + if(calibration) + { + ROS_DEBUG_STREAM("Calibration file loaded."); + config_.num_lasers = static_cast(calibration.get().num_lasers); + } + else + { + ROS_ERROR_STREAM("Could not load calibration file!"); + } + + config_.target_frame = config_.fixed_frame = "velodyne"; if(config_.organize_cloud) { - container_ptr_ = boost::shared_ptr(new OrganizedCloudXYZIR); - container_ptr_->pc->width = config_.num_lasers; - container_ptr_->pc->is_dense = false; + container_ptr_ = boost::shared_ptr( + new OrganizedCloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + config_.num_lasers, data_->scansPerPacket())); } else { - container_ptr_ = boost::shared_ptr(new PointcloudXYZIR); - container_ptr_->pc->height = 1; - container_ptr_->pc->is_dense = true; + container_ptr_ = boost::shared_ptr( + new PointcloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + data_->scansPerPacket())); } - container_ptr_->setParameters(config_.min_range, config_.max_range, config_.target_frame, config_.fixed_frame); // advertise output point cloud (before subscribing to input data) output_ = @@ -66,27 +79,31 @@ namespace velodyne_pointcloud ROS_INFO("Reconfigure Request"); data_->setParameters(config.min_range, config.max_range, config.view_direction, config.view_width); - config_.organize_cloud = config.organize_cloud; config_.min_range = config.min_range; config_.max_range = config.max_range; - - if(config_.organize_cloud)// TODO only on change - { + if(first_rcfg_call || config.organize_cloud != config_.organize_cloud){ boost::lock_guard guard(reconfigure_mtx_); - container_ptr_ = boost::shared_ptr(new OrganizedCloudXYZIR); - container_ptr_->pc->width = config_.num_lasers; - container_ptr_->pc->is_dense = false; - } - else - { - boost::lock_guard guard(reconfigure_mtx_); - container_ptr_ = boost::shared_ptr(new PointcloudXYZIR); - container_ptr_->pc->height = 1; - container_ptr_->pc->is_dense = true; + config_.organize_cloud = config.organize_cloud; + if(config_.organize_cloud) // TODO only on change + { + ROS_INFO_STREAM("Using the organized cloud format..."); + container_ptr_ = boost::shared_ptr( + new OrganizedCloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + config_.num_lasers, data_->scansPerPacket())); + } + else + { + container_ptr_ = boost::shared_ptr( + new PointcloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + data_->scansPerPacket())); + } } + first_rcfg_call = false; - container_ptr_->setParameters(config_.min_range, config_.max_range, config_.target_frame, config_.fixed_frame); + container_ptr_->configure(config_.max_range, config_.min_range, config_.fixed_frame, config_.target_frame); } @@ -96,16 +113,19 @@ namespace velodyne_pointcloud if (output_.getNumSubscribers() == 0) // no one listening? return; // avoid much work + boost::lock_guard guard(reconfigure_mtx_); // allocate a point cloud with same time and frame ID as raw data - container_ptr_->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp; - container_ptr_->pc->header.frame_id = scanMsg->header.frame_id; - container_ptr_->pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); + container_ptr_->setup(scanMsg); + // process each packet provided by the driver for (size_t i = 0; i < scanMsg->packets.size(); ++i) { data_->unpack(scanMsg->packets[i], *container_ptr_); } + // did transformation of point in the container if necessary + container_ptr_->pc->header.frame_id = config_.target_frame; + // publish the accumulated cloud message ROS_DEBUG_STREAM("Publishing " << container_ptr_->pc->height * container_ptr_->pc->width << " Velodyne points, time: " << container_ptr_->pc->header.stamp); diff --git a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc index e93001b96..bca627e47 100644 --- a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc @@ -3,11 +3,21 @@ namespace velodyne_pointcloud { + + OrganizedCloudXYZIR::OrganizedCloudXYZIR( + const double max_range, const double min_range, + const std::string& target_frame, const std::string& fixed_frame, + const unsigned int num_lasers, const unsigned int scans_per_block) + : DataContainerBase( + max_range, min_range, target_frame, fixed_frame, + num_lasers, 0, false, scans_per_block){ + } + void OrganizedCloudXYZIR::newLine() { if(config_.transform) { - computeTransformation(ros::Time(pc->header.stamp)); + computeTransformation(pcl_conversions::fromPCL(pc->header.stamp)); } for (int j = 0; j < organized_lasers.size(); j++) { @@ -39,7 +49,8 @@ namespace velodyne_pointcloud point_ptr->y = y; point_ptr->z = z; point_ptr->intensity = intensity; - } else + } + else { point_ptr->x = nan(""); point_ptr->y = nan(""); diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc index c3a3e09da..0fa942028 100644 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc @@ -5,6 +5,16 @@ namespace velodyne_pointcloud { + + PointcloudXYZIR::PointcloudXYZIR( + const double max_range, const double min_range, + const std::string& target_frame, const std::string& fixed_frame, + const unsigned int scans_per_block) + : DataContainerBase( + max_range, min_range, target_frame, fixed_frame, + 0, 1, true, scans_per_block) + {}; + void PointcloudXYZIR::newLine() { if(config_.transform) @@ -15,8 +25,8 @@ namespace velodyne_pointcloud void PointcloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& distance, const float& intensity) { - if(pointInRange(distance)) - return; + if(!pointInRange(distance)) return; + // convert polar coordinates to Euclidean XYZ velodyne_rawdata::VPoint point; point.ring = ring; diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 9ba83958e..61fddaced 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -30,8 +30,30 @@ namespace velodyne_pointcloud tf_prefix_(tf::getPrefixParam(private_nh)), data_(new velodyne_rawdata::RawData()) { - // Read calibration. - data_->setup(private_nh); + boost::optional calibration = data_->setup(private_nh); + if(calibration) + { + ROS_DEBUG_STREAM("Calibration file loaded."); + config_.num_lasers = static_cast(calibration.get().num_lasers); + } + else + { + ROS_ERROR_STREAM("Could not load calibration file!"); + } + + if(config_.organize_cloud) + { + container_ptr = boost::shared_ptr( + new OrganizedCloudXYZIR(config_.max_range, config_.min_range, config_.target_frame, config_.fixed_frame, + config_.num_lasers, data_->scansPerPacket())); + } + else + { + container_ptr = boost::shared_ptr( + new PointcloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + data_->scansPerPacket())); + } // advertise output point cloud (before subscribing to input data) output_ = @@ -51,18 +73,6 @@ namespace velodyne_pointcloud tf_filter_ptr_->registerCallback(boost::bind(&Transform::processScan, this, _1)); private_nh.param("fixed_frame", config_.fixed_frame, "odom"); - if(config_.organize_cloud) - { - container_ptr = boost::shared_ptr(new OrganizedCloudXYZIR); - container_ptr->pc->width = config_.num_lasers; - container_ptr->pc->is_dense = false; - } - else - { - container_ptr = boost::shared_ptr(new PointcloudXYZIR); - container_ptr->pc->height = 1; - container_ptr->pc->is_dense = true; - } } void Transform::reconfigure_callback( @@ -73,25 +83,29 @@ namespace velodyne_pointcloud config.view_direction, config.view_width); config_.target_frame = tf::resolve(tf_prefix_, config.frame_id); ROS_INFO_STREAM("Target frame ID: " << config_.target_frame); - config_.organize_cloud = config.organize_cloud; config_.min_range = config.min_range; config_.max_range = config.max_range; - if(config_.organize_cloud) - { - container_ptr = boost::shared_ptr(new OrganizedCloudXYZIR); - container_ptr->pc->width = config_.num_lasers; - container_ptr->pc->is_dense = false; - } - else - { - container_ptr = boost::shared_ptr(new PointcloudXYZIR); - container_ptr->pc->height = 1; - container_ptr->pc->is_dense = true; + if(config.organize_cloud != config_.organize_cloud){ + boost::lock_guard guard(reconfigure_mtx_); + config_.organize_cloud = config.organize_cloud; + if(config_.organize_cloud) + { + ROS_INFO_STREAM("Using the organized cloud format..."); + container_ptr = boost::shared_ptr( + new OrganizedCloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + config_.num_lasers, data_->scansPerPacket())); + } + else + { + container_ptr = boost::shared_ptr( + new PointcloudXYZIR(config_.max_range, config_.min_range, + config_.target_frame, config_.fixed_frame, + data_->scansPerPacket())); + } } - container_ptr->setParameters( - config_.min_range, config_.max_range, - config_.target_frame, config_.fixed_frame, listener_ptr_); + container_ptr->configure(config_.max_range, config_.min_range, config_.fixed_frame, config_.target_frame); } /** @brief Callback for raw scan messages. @@ -105,12 +119,10 @@ namespace velodyne_pointcloud if (output_.getNumSubscribers() == 0) // no one listening? return; // avoid much work - container_ptr->reset(); - container_ptr->pc->header.stamp = pcl_conversions::toPCL(scanMsg->header.stamp); - container_ptr->pc->header.frame_id = scanMsg->header.frame_id; + boost::lock_guard guard(reconfigure_mtx_); // allocate a point cloud with same time and frame ID as raw data - container_ptr->pc->points.reserve(scanMsg->packets.size() * data_->scansPerPacket()); + container_ptr->setup(scanMsg); // process each packet provided by the driver for (size_t i = 0; i < scanMsg->packets.size(); ++i) diff --git a/velodyne_pointcloud/src/lib/rawdata.cc b/velodyne_pointcloud/src/lib/rawdata.cc index a8db4c8da..4e9cc2f33 100644 --- a/velodyne_pointcloud/src/lib/rawdata.cc +++ b/velodyne_pointcloud/src/lib/rawdata.cc @@ -88,7 +88,7 @@ inline float SQR(float val) { return val*val; } } /** Set up for on-line operation. */ - int RawData::setup(ros::NodeHandle private_nh) + boost::optional RawData::setup(ros::NodeHandle private_nh) { // get path to angles.config file for this device if (!private_nh.getParam("calibration", config_.calibrationFile)) @@ -104,20 +104,20 @@ inline float SQR(float val) { return val*val; } calibration_.read(config_.calibrationFile); if (!calibration_.initialized) { - ROS_ERROR_STREAM("Unable to open calibration file: " << + ROS_ERROR_STREAM("Unable to open calibration file: " << config_.calibrationFile); - return -1; + return boost::none; } - + ROS_INFO_STREAM("Number of lasers: " << calibration_.num_lasers << "."); - + // Set up cached values for sin and cos of all the possible headings for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); } - return 0; + return calibration_; } From 2387d76ea760b07fd82521fb3197b4d397297377 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Fri, 8 Feb 2019 19:00:51 +0100 Subject: [PATCH 04/14] removed pcl to make it more elegant and faster --- velodyne_pointcloud/CMakeLists.txt | 9 +- .../include/velodyne_pointcloud/colors.h | 49 ---------- .../velodyne_pointcloud/datacontainerbase.h | 75 +++++++++------ .../organized_cloudXYZIR.h | 16 ++-- .../include/velodyne_pointcloud/point_types.h | 46 --------- .../velodyne_pointcloud/pointcloudXYZIR.h | 10 +- .../include/velodyne_pointcloud/transform.h | 10 -- velodyne_pointcloud/package.xml | 2 - .../src/conversions/CMakeLists.txt | 14 --- velodyne_pointcloud/src/conversions/colors.cc | 93 ------------------- .../src/conversions/convert.cc | 9 +- .../src/conversions/organized_cloudXYZIR.cc | 74 +++++++++------ .../src/conversions/pointcloudXYZIR.cc | 47 +++++++--- .../src/conversions/ringcolors_node.cc | 33 ------- .../src/conversions/ringcolors_nodelet.cc | 49 ---------- .../src/conversions/transform.cc | 10 +- 16 files changed, 145 insertions(+), 401 deletions(-) delete mode 100644 velodyne_pointcloud/include/velodyne_pointcloud/colors.h delete mode 100644 velodyne_pointcloud/include/velodyne_pointcloud/point_types.h delete mode 100644 velodyne_pointcloud/src/conversions/colors.cc delete mode 100644 velodyne_pointcloud/src/conversions/ringcolors_node.cc delete mode 100644 velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 049127dab..2e5aa1a9d 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -4,7 +4,6 @@ project(velodyne_pointcloud) set(${PROJECT_NAME}_CATKIN_DEPS angles nodelet - pcl_ros roscpp roslib sensor_msgs @@ -14,10 +13,9 @@ set(${PROJECT_NAME}_CATKIN_DEPS dynamic_reconfigure ) -find_package(catkin REQUIRED COMPONENTS - ${${PROJECT_NAME}_CATKIN_DEPS} pcl_conversions) +find_package(catkin REQUIRED COMPONENTS ${${PROJECT_NAME}_CATKIN_DEPS}) find_package(Boost COMPONENTS signals) - +find_package(Eigen3 REQUIRED) # Resolve system dependency on yaml-cpp, which apparently does not # provide a CMake find_package() module. find_package(PkgConfig REQUIRED) @@ -39,8 +37,9 @@ if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") add_definitions(-DHAVE_NEW_YAMLCPP) endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") -include_directories(include ${catkin_INCLUDE_DIRS} +include_directories(include ${catkin_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake + ${EIGEN3_INCLUDE_DIRS} ) catkin_package( diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/colors.h b/velodyne_pointcloud/include/velodyne_pointcloud/colors.h deleted file mode 100644 index a88e308ef..000000000 --- a/velodyne_pointcloud/include/velodyne_pointcloud/colors.h +++ /dev/null @@ -1,49 +0,0 @@ -/* -*- mode: C++ -*- */ -/* - * Copyright (C) 2012 Austin Robot Technology - * License: Modified BSD Software License Agreement - * - * $Id$ - */ - -/** @file - - Interface for converting a Velodyne 3D LIDAR PointXYZIR cloud to - PointXYZRGB, assigning colors for visualization of the laser - rings. - - @author Jack O'Quin -*/ - -#ifndef _VELODYNE_POINTCLOUD_COLORS_H_ -#define _VELODYNE_POINTCLOUD_COLORS_H_ - -#include -#include -#include -#include - -namespace velodyne_pointcloud -{ - // shorter names for point cloud types in this namespace - typedef velodyne_pointcloud::PointXYZIR VPoint; - typedef pcl::PointCloud VPointCloud; - - class RingColors - { - public: - - RingColors(ros::NodeHandle node, ros::NodeHandle private_nh); - ~RingColors() {} - - private: - - void convertPoints(const VPointCloud::ConstPtr &inMsg); - - ros::Subscriber input_; - ros::Publisher output_; - }; - -} // namespace velodyne_pointcloud - -#endif // _VELODYNE_POINTCLOUD_COLORS_H_ diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index a48c289be..03379f8fd 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -1,17 +1,14 @@ #ifndef __DATACONTAINERBASE_H #define __DATACONTAINERBASE_H -#include -#include #include #include +#include +#include +#include namespace velodyne_rawdata { - // Shorthand typedefs for point cloud representations - typedef velodyne_pointcloud::PointXYZIR VPoint; - typedef pcl::PointCloud VPointCloud; - class DataContainerBase { public: @@ -19,10 +16,25 @@ namespace velodyne_rawdata const double max_range, const double min_range, const std::string& target_frame, const std::string& fixed_frame, const unsigned int init_width, const unsigned int init_height, - const bool is_dense, const unsigned int scans_per_packet) + const bool is_dense, const unsigned int scans_per_packet, int fields, ...) : config_(max_range, min_range, target_frame, fixed_frame, - init_width, init_height, is_dense, scans_per_packet), pc(new VPointCloud) { - + init_width, init_height, is_dense, scans_per_packet) { + + va_list vl; + cloud.fields.clear(); + cloud.fields.reserve(fields); + va_start(vl, fields); + int offset = 0; + for (int i = 0; i < fields; ++i) { + // Create the corresponding PointField + std::string name(va_arg(vl, char*)); + int count(va_arg(vl, int)); + int datatype(va_arg(vl, int)); + offset = addPointField(cloud, name, count, datatype, offset); + } + va_end(vl); + cloud.point_step = offset; + cloud.row_step = cloud.width * cloud.point_step; if(config_.transform) { tf_ptr = boost::shared_ptr(new tf::TransformListener); @@ -60,22 +72,29 @@ namespace velodyne_rawdata } }; - void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ - pc = VPointCloud::Ptr(new VPointCloud); - pc->header.stamp = pcl_conversions::toPCL(scan_msg->header.stamp); - pc->header.frame_id = scan_msg->header.frame_id; - pc->points.reserve(scan_msg->packets.size() * config_.scans_per_packet); - pc->width = config_.init_width; - pc->height = config_.init_height; - pc->is_dense = config_.is_dense; + virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ + cloud.header = scan_msg->header; + cloud.data.resize(scan_msg->packets.size() * config_.scans_per_packet * cloud.point_step); + cloud.width = config_.init_width; + cloud.height = config_.init_height; + cloud.is_dense = static_cast(config_.is_dense); } virtual void addPoint( - const float& x, const float& y, const float& z, - const uint16_t& ring, const uint16_t& azimuth, - const float& distance, const float& intensity) = 0; + float x, float y, float z, + const uint16_t ring, const uint16_t azimuth, + const float distance, const float intensity) = 0; virtual void newLine() = 0; + const sensor_msgs::PointCloud2& finishCloud(){ + cloud.data.resize(cloud.point_step * cloud.width * cloud.height); + cloud.header.frame_id = config_.target_frame; + ROS_DEBUG_STREAM("Prepared cloud width" << cloud.height * cloud.width + << " Velodyne points, time: " << cloud.header.stamp); + + return cloud; + } + void configure(const double max_range, const double min_range, const std::string fixed_frame, const std::string target_frame) { config_.max_range = max_range; @@ -89,7 +108,7 @@ namespace velodyne_rawdata } } - VPointCloud::Ptr pc; + sensor_msgs::PointCloud2 cloud; protected: @@ -102,7 +121,7 @@ namespace velodyne_rawdata bool computeTransformation(const ros::Time& time){ tf::StampedTransform transform; try{ - tf_ptr->lookupTransform(config_.target_frame, pc->header.frame_id, time, transform); + tf_ptr->lookupTransform(config_.target_frame, cloud.header.frame_id, time, transform); } catch (tf::LookupException &e){ ROS_ERROR ("%s", e.what ()); @@ -129,13 +148,10 @@ namespace velodyne_rawdata return true; } - inline bool transformPoint(velodyne_rawdata::VPoint& point) + void transformPoint(float& x, float& y, float& z) { - Eigen::Vector3d vec(point.x, point.y, point.z), out_vec; - out_vec = transformation * vec; - point.x = out_vec(0); - point.y = out_vec(1); - point.z = out_vec(2); + Eigen::Vector3d p = transformation * Eigen::Vector3d(x, y, z); + x = p.x(); y = p.y(); z = p.z(); } inline bool pointInRange(float range) @@ -145,10 +161,9 @@ namespace velodyne_rawdata } Config config_; - boost::shared_ptr tf_ptr; ///< transform listener - Eigen::Affine3d transformation; + }; } #endif //__DATACONTAINERBASE_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h index 2c2556ccf..383725dac 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h @@ -2,6 +2,7 @@ #define __ORGANIZED_CLOUDXYZIR_H #include +#include namespace velodyne_pointcloud { @@ -16,17 +17,16 @@ namespace velodyne_pointcloud virtual void newLine(); + virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); + virtual void addPoint( - const float& x, - const float& y, - const float& z, - const uint16_t& ring, - const uint16_t& azimuth, - const float& distance, - const float& intensity); + float x, float y, float z, + const uint16_t ring, const uint16_t azimuth, + const float distance, const float intensity); private: - std::map organized_lasers; + sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity; + sensor_msgs::PointCloud2Iterator iter_ring; }; } #endif //__ORGANIZED_CLOUDXYZIR_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h b/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h deleted file mode 100644 index 75a75e280..000000000 --- a/velodyne_pointcloud/include/velodyne_pointcloud/point_types.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -*- mode: C++ -*- - * - * Copyright (C) 2011, 2012 Austin Robot Technology - * - * License: Modified BSD Software License Agreement - * - * $Id: data_base.h 1554 2011-06-14 22:11:17Z jack.oquin $ - */ - -/** \file - * - * Point Cloud Library point structures for Velodyne data. - * - * @author Jesse Vera - * @author Jack O'Quin - * @author Piyush Khandelwal - */ - -#ifndef __VELODYNE_POINTCLOUD_POINT_TYPES_H -#define __VELODYNE_POINTCLOUD_POINT_TYPES_H - -#include - -namespace velodyne_pointcloud -{ - /** Euclidean Velodyne coordinate, including intensity and ring number. */ - struct PointXYZIR - { - PCL_ADD_POINT4D; // quad-word XYZ - float intensity; ///< laser intensity reading - uint16_t ring; ///< laser ring number - EIGEN_MAKE_ALIGNED_OPERATOR_NEW // ensure proper alignment - } EIGEN_ALIGN16; - -}; // namespace velodyne_pointcloud - - -POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_pointcloud::PointXYZIR, - (float, x, x) - (float, y, y) - (float, z, z) - (float, intensity, intensity) - (uint16_t, ring, ring)) - -#endif // __VELODYNE_POINTCLOUD_POINT_TYPES_H - diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h index 3572b5384..727576971 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h @@ -18,9 +18,13 @@ namespace velodyne_pointcloud virtual void newLine(); - virtual void addPoint(const float& x, const float& y, const float& z, - const uint16_t& ring, const uint16_t& azimuth, - const float& distance, const float& intensity); + virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); + + virtual void addPoint(float x, float y, float z, + uint16_t ring, uint16_t azimuth, float distance, float intensity); + + sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity; + sensor_msgs::PointCloud2Iterator iter_ring; }; } diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h index d464f25d2..89616cce9 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -29,16 +29,6 @@ #include #include -// include template implementations to transform a custom point cloud -#include - -// instantiate template for transforming a VPointCloud -template bool - pcl_ros::transformPointCloud(const std::string &, - const velodyne_rawdata::VPointCloud &, - velodyne_rawdata::VPointCloud &, - const tf::TransformListener &); - namespace velodyne_pointcloud { class Transform diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 6fea49f4c..1c1be705f 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -18,8 +18,6 @@ angles nodelet - pcl_conversions - pcl_ros pluginlib roscpp roslib diff --git a/velodyne_pointcloud/src/conversions/CMakeLists.txt b/velodyne_pointcloud/src/conversions/CMakeLists.txt index 54d8240ed..6e846ce4f 100644 --- a/velodyne_pointcloud/src/conversions/CMakeLists.txt +++ b/velodyne_pointcloud/src/conversions/CMakeLists.txt @@ -14,20 +14,6 @@ install(TARGETS cloud_nodelet ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -add_executable(ringcolors_node ringcolors_node.cc colors.cc) -target_link_libraries(ringcolors_node - ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) -install(TARGETS ringcolors_node - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -add_library(ringcolors_nodelet ringcolors_nodelet.cc colors.cc) -target_link_libraries(ringcolors_nodelet - ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) -install(TARGETS ringcolors_nodelet - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - add_executable(transform_node transform_node.cc transform.cc pointcloudXYZIR.cc organized_cloudXYZIR.cc) add_dependencies(transform_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(transform_node velodyne_rawdata diff --git a/velodyne_pointcloud/src/conversions/colors.cc b/velodyne_pointcloud/src/conversions/colors.cc deleted file mode 100644 index 03d3f1b86..000000000 --- a/velodyne_pointcloud/src/conversions/colors.cc +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Copyright (C) 2012 Austin Robot Technology - * License: Modified BSD Software License Agreement - * - * $Id$ - */ - -/** @file - - Implementation for converting a Velodyne 3D LIDAR PointXYZIR cloud - to PointXYZRGB, assigning colors for visualization of the laser - rings. - - @author Jack O'Quin -*/ - -#include "velodyne_pointcloud/colors.h" -#include - -/// @todo make sure these includes are really necessary -#include -#include - -namespace -{ - // RGB color values - const int color_red = 0xff0000; - const int color_orange = 0xff8800; - const int color_yellow = 0xffff00; - const int color_green = 0x00ff00; - const int color_blue = 0x0000ff; - const int color_violet = 0xff00ff; - - const int N_COLORS = 6; - int rainbow[N_COLORS] = {color_red, color_orange, color_yellow, - color_green, color_blue, color_violet}; -} - -namespace velodyne_pointcloud -{ - - /** types of output point and cloud */ - typedef pcl::PointXYZRGB RGBPoint; - typedef pcl::PointCloud RGBPointCloud; - - /** @brief Constructor. */ - RingColors::RingColors(ros::NodeHandle node, ros::NodeHandle private_nh) - { - // advertise output point cloud (before subscribing to input data) - output_ = - node.advertise("velodyne_rings", 10); - - // subscribe to VelodyneScan packets - input_ = - node.subscribe("velodyne_points", 10, - &RingColors::convertPoints, this, - ros::TransportHints().tcpNoDelay(true)); - } - - - /** @brief Callback for Velodyne PointXYZRI messages. */ - void - RingColors::convertPoints(const VPointCloud::ConstPtr &inMsg) - { - if (output_.getNumSubscribers() == 0) // no one listening? - return; // do nothing - - // allocate an PointXYZRGB message with same time and frame ID as - // input data - RGBPointCloud::Ptr outMsg(new RGBPointCloud()); - outMsg->header.stamp = inMsg->header.stamp; - outMsg->header.frame_id = inMsg->header.frame_id; - outMsg->height = 1; - - for (size_t i = 0; i < inMsg->points.size(); ++i) - { - RGBPoint p; - p.x = inMsg->points[i].x; - p.y = inMsg->points[i].y; - p.z = inMsg->points[i].z; - - // color lasers with the rainbow array - int color = inMsg->points[i].ring % N_COLORS; - p.rgb = *reinterpret_cast(rainbow+color); - - outMsg->points.push_back(p); - ++outMsg->width; - } - - output_.publish(outMsg); - } - -} // namespace velodyne_pcl diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index 0644a4957..ac60b66d0 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -17,7 +17,6 @@ #include #include -#include namespace velodyne_pointcloud { @@ -123,13 +122,7 @@ namespace velodyne_pointcloud data_->unpack(scanMsg->packets[i], *container_ptr_); } - // did transformation of point in the container if necessary - container_ptr_->pc->header.frame_id = config_.target_frame; - - // publish the accumulated cloud message - ROS_DEBUG_STREAM("Publishing " << container_ptr_->pc->height * container_ptr_->pc->width - << " Velodyne points, time: " << container_ptr_->pc->header.stamp); - output_.publish(container_ptr_->pc); + output_.publish(container_ptr_->finishCloud()); } } // namespace velodyne_pointcloud diff --git a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc index bca627e47..dc5ee2140 100644 --- a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc @@ -10,53 +10,71 @@ namespace velodyne_pointcloud const unsigned int num_lasers, const unsigned int scans_per_block) : DataContainerBase( max_range, min_range, target_frame, fixed_frame, - num_lasers, 0, false, scans_per_block){ + num_lasers, 0, false, scans_per_block, 5, + "x", 1, sensor_msgs::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "intensity", 1, sensor_msgs::PointField::FLOAT32, + "ring", 1, sensor_msgs::PointField::UINT16), + iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"), + iter_intensity(cloud, "intensity"), iter_ring(cloud, "ring") + { } void OrganizedCloudXYZIR::newLine() { + ++cloud.height; if(config_.transform) { - computeTransformation(pcl_conversions::fromPCL(pc->header.stamp)); + //TODO transform with time associated with the point line + computeTransformation(cloud.header.stamp); } - for (int j = 0; j < organized_lasers.size(); j++) - { - velodyne_rawdata::VPoint *point = organized_lasers[j]; - if(config_.transform) - { - transformPoint(*point); - } - pc->points.push_back(*point); - delete point; - } - // append this point to the cloud - ++pc->height; } - void OrganizedCloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& distance, const float& intensity) + void OrganizedCloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ + DataContainerBase::setup(scan_msg); + iter_x = sensor_msgs::PointCloud2Iterator(cloud, "x"); + iter_y = sensor_msgs::PointCloud2Iterator(cloud, "y"); + iter_z = sensor_msgs::PointCloud2Iterator(cloud, "z"); + iter_intensity = sensor_msgs::PointCloud2Iterator(cloud, "intensity"); + iter_ring = sensor_msgs::PointCloud2Iterator(cloud, "ring"); + } + + + void OrganizedCloudXYZIR::addPoint(float x, float y, float z, + const uint16_t ring, const uint16_t /*azimuth*/, const float distance, const float intensity) { - velodyne_rawdata::VPoint *point_ptr = new velodyne_rawdata::VPoint; /** The laser values are not ordered, the organized structure * needs ordered neighbour points. The right order is defined - * by the laser_ring value. First collect each laser point - * and insert them into a map, ordered by the ring number + * by the laser_ring value. + * To keep the right ordering, the filtered values are set to + * NaN. */ - organized_lasers[ring] = point_ptr; - point_ptr->ring = ring; if (pointInRange(distance)) { - point_ptr->x = x; - point_ptr->y = y; - point_ptr->z = z; - point_ptr->intensity = intensity; + if(config_.transform) + transformPoint(x, y, z); + + *(iter_x+ring) = x; + *(iter_y+ring) = y; + *(iter_z+ring) = z; + *(iter_intensity+ring) = intensity; + *(iter_ring+ring) = ring; } else { - point_ptr->x = nan(""); - point_ptr->y = nan(""); - point_ptr->z = nan(""); - point_ptr->intensity = 0; + *(iter_x+ring) = nanf(""); + *(iter_y+ring) = nanf(""); + *(iter_z+ring) = nanf(""); + *(iter_intensity+ring) = nanf(""); + *(iter_ring+ring) = ring; } + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_ring; + ++iter_intensity; } } diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc index 0fa942028..ca6a7d07b 100644 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc @@ -12,37 +12,54 @@ namespace velodyne_pointcloud const unsigned int scans_per_block) : DataContainerBase( max_range, min_range, target_frame, fixed_frame, - 0, 1, true, scans_per_block) + 0, 1, true, scans_per_block, 5, + "x", 1, sensor_msgs::PointField::FLOAT32, + "y", 1, sensor_msgs::PointField::FLOAT32, + "z", 1, sensor_msgs::PointField::FLOAT32, + "intensity", 1, sensor_msgs::PointField::FLOAT32, + "ring", 1, sensor_msgs::PointField::UINT16), + iter_x(cloud, "x"), iter_y(cloud, "y"), iter_z(cloud, "z"), + iter_ring(cloud, "ring"), iter_intensity(cloud, "intensity") {}; + void PointcloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ + DataContainerBase::setup(scan_msg); + iter_x = sensor_msgs::PointCloud2Iterator(cloud, "x"); + iter_y = sensor_msgs::PointCloud2Iterator(cloud, "y"); + iter_z = sensor_msgs::PointCloud2Iterator(cloud, "z"); + iter_intensity = sensor_msgs::PointCloud2Iterator(cloud, "intensity"); + iter_ring = sensor_msgs::PointCloud2Iterator(cloud, "ring"); + } + void PointcloudXYZIR::newLine() { if(config_.transform) { - computeTransformation(pcl_conversions::fromPCL(pc->header.stamp)); + computeTransformation(cloud.header.stamp); } } - void PointcloudXYZIR::addPoint(const float& x, const float& y, const float& z, const uint16_t& ring, const uint16_t& /*azimuth*/, const float& distance, const float& intensity) + void PointcloudXYZIR::addPoint(float x, float y, float z, uint16_t ring, uint16_t /*azimuth*/, float distance, float intensity) { if(!pointInRange(distance)) return; // convert polar coordinates to Euclidean XYZ - velodyne_rawdata::VPoint point; - point.ring = ring; - point.x = x; - point.y = y; - point.z = z; - point.intensity = intensity; if(config_.transform) - { - transformPoint(point); - } + transformPoint(x, y, z); + + *iter_x = x; + *iter_y = y; + *iter_z = z; + *iter_ring = ring; + *iter_intensity = intensity; - // append this point to the cloud - pc->points.push_back(point); - ++pc->width; + ++cloud.width; + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_ring; + ++iter_intensity; } } diff --git a/velodyne_pointcloud/src/conversions/ringcolors_node.cc b/velodyne_pointcloud/src/conversions/ringcolors_node.cc deleted file mode 100644 index 32292f28e..000000000 --- a/velodyne_pointcloud/src/conversions/ringcolors_node.cc +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin - * License: Modified BSD Software License Agreement - * - * $Id$ - */ - -/** @file - - This ROS node converts a Velodyne 3D LIDAR PointXYZIR cloud to - PointXYZRGB, assigning colors for visualization of the laser - rings. - -*/ - -#include -#include "velodyne_pointcloud/colors.h" - -/** Main node entry point. */ -int main(int argc, char **argv) -{ - ros::init(argc, argv, "colors_node"); - ros::NodeHandle node; - ros::NodeHandle priv_nh("~"); - - // create conversion class, which subscribes to input messages - velodyne_pointcloud::RingColors colors(node, priv_nh); - - // handle callbacks until shut down - ros::spin(); - - return 0; -} diff --git a/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc b/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc deleted file mode 100644 index 39cfd18c0..000000000 --- a/velodyne_pointcloud/src/conversions/ringcolors_nodelet.cc +++ /dev/null @@ -1,49 +0,0 @@ -/* - * Copyright (C) 2012 Austin Robot Technology, Jack O'Quin - * License: Modified BSD Software License Agreement - * - * $Id$ - */ - -/** @file - - This ROS nodelet converts a Velodyne 3D LIDAR PointXYZIR cloud to - PointXYZRGB, assigning colors for visualization of the laser - rings. - -*/ - -#include -#include -#include - -#include "velodyne_pointcloud/colors.h" - -namespace velodyne_pointcloud -{ - class RingColorsNodelet: public nodelet::Nodelet - { - public: - - RingColorsNodelet() {} - ~RingColorsNodelet() {} - - private: - - virtual void onInit(); - boost::shared_ptr colors_; - }; - - /** @brief Nodelet initialization. */ - void RingColorsNodelet::onInit() - { - colors_.reset(new RingColors(getNodeHandle(), getPrivateNodeHandle())); - } - -} // namespace velodyne_pointcloud - - -// Register this plugin with pluginlib. Names must match nodelets.xml. -// -// parameters: class type, base class type -PLUGINLIB_EXPORT_CLASS(velodyne_pointcloud::RingColorsNodelet, nodelet::Nodelet) diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 61fddaced..7f6857853 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -14,6 +14,7 @@ @author Jack O'Quin @author Jesse Vera + @author Sebastian Pütz */ @@ -21,7 +22,6 @@ #include #include -#include namespace velodyne_pointcloud { @@ -129,13 +129,7 @@ namespace velodyne_pointcloud { data_->unpack(scanMsg->packets[i], *container_ptr); } - // did transformation of point in the container if necessary - container_ptr->pc->header.frame_id = config_.target_frame; - - // publish the accumulated cloud message - ROS_DEBUG_STREAM("Publishing " << container_ptr->pc->height * container_ptr->pc->width - << " Velodyne points, time: " << container_ptr->pc->header.stamp); - output_.publish(container_ptr->pc); + output_.publish(container_ptr->finishCloud()); } } // namespace velodyne_pointcloud From 7f52b3586a2833b4a48bd2539a862adedb161baf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Sat, 9 Feb 2019 17:26:14 +0100 Subject: [PATCH 05/14] fix Eigen include --- .../include/velodyne_pointcloud/datacontainerbase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index 03379f8fd..af3033b35 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include namespace velodyne_rawdata From 3c47db6a91fe4091ce9b84ea7487499e0e793aaa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Sat, 9 Feb 2019 17:46:06 +0100 Subject: [PATCH 06/14] fix pc2 iter line increment --- .../src/conversions/organized_cloudXYZIR.cc | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc index dc5ee2140..95609b3ea 100644 --- a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc @@ -23,6 +23,11 @@ namespace velodyne_pointcloud void OrganizedCloudXYZIR::newLine() { + iter_x = iter_x + config_.init_width; + iter_y = iter_y + config_.init_width; + iter_z = iter_z + config_.init_width; + iter_ring = iter_ring + config_.init_width; + iter_intensity = iter_intensity + config_.init_width; ++cloud.height; if(config_.transform) { @@ -69,12 +74,6 @@ namespace velodyne_pointcloud *(iter_intensity+ring) = nanf(""); *(iter_ring+ring) = ring; } - - ++iter_x; - ++iter_y; - ++iter_z; - ++iter_ring; - ++iter_intensity; } } From b590d0060cd187d20f9b45aa20d833773382b081 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Mon, 11 Feb 2019 09:41:00 -0600 Subject: [PATCH 07/14] Adding Eigen as rosdep for pointcloud. --- velodyne_pointcloud/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 84dd30fe7..84937b81f 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -27,6 +27,7 @@ velodyne_msgs yaml-cpp dynamic_reconfigure + eigen velodyne_laserscan From 62c9aec89da2824d9a6ec63856cb533af25e2f06 Mon Sep 17 00:00:00 2001 From: Joshua Whitley Date: Mon, 11 Feb 2019 09:58:27 -0600 Subject: [PATCH 08/14] Launch files lost execute permissions. --- velodyne_pointcloud/launch/32e_points.launch | 0 velodyne_pointcloud/launch/VLP-32C_points.launch | 0 velodyne_pointcloud/launch/VLP16_points.launch | 0 velodyne_pointcloud/launch/cloud_nodelet.launch | 0 velodyne_pointcloud/launch/laserscan_nodelet.launch | 0 velodyne_pointcloud/launch/transform_nodelet.launch | 0 6 files changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 velodyne_pointcloud/launch/32e_points.launch mode change 100644 => 100755 velodyne_pointcloud/launch/VLP-32C_points.launch mode change 100644 => 100755 velodyne_pointcloud/launch/VLP16_points.launch mode change 100644 => 100755 velodyne_pointcloud/launch/cloud_nodelet.launch mode change 100644 => 100755 velodyne_pointcloud/launch/laserscan_nodelet.launch mode change 100644 => 100755 velodyne_pointcloud/launch/transform_nodelet.launch diff --git a/velodyne_pointcloud/launch/32e_points.launch b/velodyne_pointcloud/launch/32e_points.launch old mode 100644 new mode 100755 diff --git a/velodyne_pointcloud/launch/VLP-32C_points.launch b/velodyne_pointcloud/launch/VLP-32C_points.launch old mode 100644 new mode 100755 diff --git a/velodyne_pointcloud/launch/VLP16_points.launch b/velodyne_pointcloud/launch/VLP16_points.launch old mode 100644 new mode 100755 diff --git a/velodyne_pointcloud/launch/cloud_nodelet.launch b/velodyne_pointcloud/launch/cloud_nodelet.launch old mode 100644 new mode 100755 diff --git a/velodyne_pointcloud/launch/laserscan_nodelet.launch b/velodyne_pointcloud/launch/laserscan_nodelet.launch old mode 100644 new mode 100755 diff --git a/velodyne_pointcloud/launch/transform_nodelet.launch b/velodyne_pointcloud/launch/transform_nodelet.launch old mode 100644 new mode 100755 From 0146ae9130405c1f326b7d50dc2ae3d92d4d0a1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Mon, 11 Feb 2019 17:30:34 +0100 Subject: [PATCH 09/14] remove gdb launch prefix --- velodyne_driver/launch/nodelet_manager.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_driver/launch/nodelet_manager.launch b/velodyne_driver/launch/nodelet_manager.launch index 64e1886fe..835e50dbd 100644 --- a/velodyne_driver/launch/nodelet_manager.launch +++ b/velodyne_driver/launch/nodelet_manager.launch @@ -16,7 +16,7 @@ - + Date: Tue, 12 Feb 2019 14:56:17 +0100 Subject: [PATCH 10/14] fix tf listener init --- .../velodyne_pointcloud/datacontainerbase.h | 27 ++++++++++++------- .../organized_cloudXYZIR.h | 4 ++- .../velodyne_pointcloud/pointcloudXYZIR.h | 4 ++- .../include/velodyne_pointcloud/transform.h | 3 ++- .../src/conversions/convert.cc | 3 +-- .../src/conversions/organized_cloudXYZIR.cc | 10 +++---- .../src/conversions/pointcloudXYZIR.cc | 11 +++----- .../src/conversions/transform.cc | 24 ++++++++++------- 8 files changed, 47 insertions(+), 39 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index af3033b35..1e6f1ba97 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -16,9 +16,11 @@ namespace velodyne_rawdata const double max_range, const double min_range, const std::string& target_frame, const std::string& fixed_frame, const unsigned int init_width, const unsigned int init_height, - const bool is_dense, const unsigned int scans_per_packet, int fields, ...) + const bool is_dense, const unsigned int scans_per_packet, + boost::shared_ptr& tf_ptr, + int fields, ...) : config_(max_range, min_range, target_frame, fixed_frame, - init_width, init_height, is_dense, scans_per_packet) { + init_width, init_height, is_dense, scans_per_packet), tf_ptr(tf_ptr) { va_list vl; cloud.fields.clear(); @@ -35,7 +37,7 @@ namespace velodyne_rawdata va_end(vl); cloud.point_step = offset; cloud.row_step = cloud.width * cloud.point_step; - if(config_.transform) + if(config_.transform && !tf_ptr) { tf_ptr = boost::shared_ptr(new tf::TransformListener); } @@ -78,6 +80,12 @@ namespace velodyne_rawdata cloud.width = config_.init_width; cloud.height = config_.init_height; cloud.is_dense = static_cast(config_.is_dense); + if(config_.transform) + { + if(!computeTransformation(scan_msg->header.stamp)){ + ROS_ERROR_STREAM("Could not transform points!"); + } + } } virtual void addPoint( @@ -101,8 +109,9 @@ namespace velodyne_rawdata config_.min_range = min_range; config_.fixed_frame = fixed_frame; config_.target_frame = target_frame; - config_.transform = fixed_frame != target_frame; - if(config_.transform) + + config_.transform = fixed_frame.compare(target_frame) != 0; + if(config_.transform && !tf_ptr) { tf_ptr = boost::shared_ptr(new tf::TransformListener); } @@ -118,7 +127,7 @@ namespace velodyne_rawdata eigen_vec(2) = tf_vec[2]; } - bool computeTransformation(const ros::Time& time){ + inline bool computeTransformation(const ros::Time& time){ tf::StampedTransform transform; try{ tf_ptr->lookupTransform(config_.target_frame, cloud.header.frame_id, time, transform); @@ -140,15 +149,14 @@ namespace velodyne_rawdata quaternion.z () ); - tf::Vector3 origin = transform.getOrigin (); Eigen::Vector3d eigen_origin; - vectorTfToEigen(origin, eigen_origin); + vectorTfToEigen(transform.getOrigin(), eigen_origin); Eigen::Translation3d translation ( eigen_origin ); transformation = translation * rotation; return true; } - void transformPoint(float& x, float& y, float& z) + inline void transformPoint(float& x, float& y, float& z) { Eigen::Vector3d p = transformation * Eigen::Vector3d(x, y, z); x = p.x(); y = p.y(); z = p.z(); @@ -163,7 +171,6 @@ namespace velodyne_rawdata Config config_; boost::shared_ptr tf_ptr; ///< transform listener Eigen::Affine3d transformation; - }; } #endif //__DATACONTAINERBASE_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h index 383725dac..a9b492a2b 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h @@ -13,7 +13,9 @@ namespace velodyne_pointcloud OrganizedCloudXYZIR( const double max_range, const double min_range, const std::string& target_frame, const std::string& fixed_frame, - const unsigned int num_lasers, const unsigned int scans_per_block); + const unsigned int num_lasers, const unsigned int scans_per_block, + boost::shared_ptr tf_ptr = boost::shared_ptr() + ); virtual void newLine(); diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h index 727576971..286d0eecc 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h @@ -14,7 +14,9 @@ namespace velodyne_pointcloud PointcloudXYZIR( const double max_range, const double min_range, const std::string& target_frame, const std::string& fixed_frame, - const unsigned int scans_per_block); + const unsigned int scans_per_block, + boost::shared_ptr tf_ptr = boost::shared_ptr() + ); virtual void newLine(); diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h index 89616cce9..3a5b88550 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -53,7 +53,7 @@ namespace velodyne_pointcloud message_filters::Subscriber velodyne_scan_; ros::Publisher output_; boost::shared_ptr >tf_filter_ptr_; - boost::shared_ptr listener_ptr_; + boost::shared_ptr tf_ptr_; /// configuration parameters typedef struct { @@ -65,6 +65,7 @@ namespace velodyne_pointcloud uint16_t num_lasers; ///< number of lasers } Config; Config config_; + bool first_rcfg_call; boost::shared_ptr container_ptr; diff --git a/velodyne_pointcloud/src/conversions/convert.cc b/velodyne_pointcloud/src/conversions/convert.cc index ac60b66d0..04955ca41 100644 --- a/velodyne_pointcloud/src/conversions/convert.cc +++ b/velodyne_pointcloud/src/conversions/convert.cc @@ -82,7 +82,7 @@ namespace velodyne_pointcloud config_.max_range = config.max_range; if(first_rcfg_call || config.organize_cloud != config_.organize_cloud){ - boost::lock_guard guard(reconfigure_mtx_); + first_rcfg_call = false; config_.organize_cloud = config.organize_cloud; if(config_.organize_cloud) // TODO only on change { @@ -100,7 +100,6 @@ namespace velodyne_pointcloud data_->scansPerPacket())); } } - first_rcfg_call = false; container_ptr_->configure(config_.max_range, config_.min_range, config_.fixed_frame, config_.target_frame); diff --git a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc index 95609b3ea..f98fdb1ed 100644 --- a/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/organized_cloudXYZIR.cc @@ -7,10 +7,11 @@ namespace velodyne_pointcloud OrganizedCloudXYZIR::OrganizedCloudXYZIR( const double max_range, const double min_range, const std::string& target_frame, const std::string& fixed_frame, - const unsigned int num_lasers, const unsigned int scans_per_block) + const unsigned int num_lasers, const unsigned int scans_per_block, + boost::shared_ptr tf_ptr) : DataContainerBase( max_range, min_range, target_frame, fixed_frame, - num_lasers, 0, false, scans_per_block, 5, + num_lasers, 0, false, scans_per_block, tf_ptr, 5, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, @@ -29,11 +30,6 @@ namespace velodyne_pointcloud iter_ring = iter_ring + config_.init_width; iter_intensity = iter_intensity + config_.init_width; ++cloud.height; - if(config_.transform) - { - //TODO transform with time associated with the point line - computeTransformation(cloud.header.stamp); - } } void OrganizedCloudXYZIR::setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ diff --git a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc index ca6a7d07b..300755e15 100644 --- a/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc +++ b/velodyne_pointcloud/src/conversions/pointcloudXYZIR.cc @@ -9,10 +9,10 @@ namespace velodyne_pointcloud PointcloudXYZIR::PointcloudXYZIR( const double max_range, const double min_range, const std::string& target_frame, const std::string& fixed_frame, - const unsigned int scans_per_block) + const unsigned int scans_per_block, boost::shared_ptr tf_ptr) : DataContainerBase( max_range, min_range, target_frame, fixed_frame, - 0, 1, true, scans_per_block, 5, + 0, 1, true, scans_per_block, tf_ptr, 5, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, @@ -32,12 +32,7 @@ namespace velodyne_pointcloud } void PointcloudXYZIR::newLine() - { - if(config_.transform) - { - computeTransformation(cloud.header.stamp); - } - } + {} void PointcloudXYZIR::addPoint(float x, float y, float z, uint16_t ring, uint16_t /*azimuth*/, float distance, float intensity) { diff --git a/velodyne_pointcloud/src/conversions/transform.cc b/velodyne_pointcloud/src/conversions/transform.cc index 7f6857853..18176e522 100644 --- a/velodyne_pointcloud/src/conversions/transform.cc +++ b/velodyne_pointcloud/src/conversions/transform.cc @@ -28,7 +28,8 @@ namespace velodyne_pointcloud /** @brief Constructor. */ Transform::Transform(ros::NodeHandle node, ros::NodeHandle private_nh): tf_prefix_(tf::getPrefixParam(private_nh)), - data_(new velodyne_rawdata::RawData()) + data_(new velodyne_rawdata::RawData), + first_rcfg_call(true) { boost::optional calibration = data_->setup(private_nh); if(calibration) @@ -41,18 +42,21 @@ namespace velodyne_pointcloud ROS_ERROR_STREAM("Could not load calibration file!"); } + config_.target_frame = config_.fixed_frame = "velodyne"; + tf_ptr_ = boost::make_shared(); + if(config_.organize_cloud) { container_ptr = boost::shared_ptr( new OrganizedCloudXYZIR(config_.max_range, config_.min_range, config_.target_frame, config_.fixed_frame, - config_.num_lasers, data_->scansPerPacket())); + config_.num_lasers, data_->scansPerPacket(), tf_ptr_)); } else { container_ptr = boost::shared_ptr( new PointcloudXYZIR(config_.max_range, config_.min_range, config_.target_frame, config_.fixed_frame, - data_->scansPerPacket())); + data_->scansPerPacket(), tf_ptr_)); } // advertise output point cloud (before subscribing to input data) @@ -65,11 +69,11 @@ namespace velodyne_pointcloud CallbackType f; f = boost::bind (&Transform::reconfigure_callback, this, _1, _2); srv_->setCallback (f); - + // subscribe to VelodyneScan packets using transform filter velodyne_scan_.subscribe(node, "velodyne_packets", 10); tf_filter_ptr_ = boost::shared_ptr >( - new tf::MessageFilter(velodyne_scan_, *listener_ptr_, config_.target_frame, 10)); + new tf::MessageFilter(velodyne_scan_, *tf_ptr_, config_.target_frame, 10)); tf_filter_ptr_->registerCallback(boost::bind(&Transform::processScan, this, _1)); private_nh.param("fixed_frame", config_.fixed_frame, "odom"); @@ -79,15 +83,17 @@ namespace velodyne_pointcloud velodyne_pointcloud::TransformNodeConfig &config, uint32_t level) { ROS_INFO_STREAM("Reconfigure request."); - data_->setParameters(config.min_range, config.max_range, + data_->setParameters(config.min_range, config.max_range, config.view_direction, config.view_width); config_.target_frame = tf::resolve(tf_prefix_, config.frame_id); - ROS_INFO_STREAM("Target frame ID: " << config_.target_frame); + ROS_INFO_STREAM("Target frame ID now: " << config_.target_frame); config_.min_range = config.min_range; config_.max_range = config.max_range; - if(config.organize_cloud != config_.organize_cloud){ - boost::lock_guard guard(reconfigure_mtx_); + boost::lock_guard guard(reconfigure_mtx_); + + if(first_rcfg_call || config.organize_cloud != config_.organize_cloud){ + first_rcfg_call = false; config_.organize_cloud = config.organize_cloud; if(config_.organize_cloud) { From c1e778ae2f04126131f0d15dd7f010ca05d73861 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sebastian=20P=C3=BCtz?= Date: Tue, 12 Feb 2019 15:14:11 +0100 Subject: [PATCH 11/14] removed unconfigured and unused build depend --- velodyne_driver/package.xml | 1 - velodyne_pointcloud/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/velodyne_driver/package.xml b/velodyne_driver/package.xml index a85302fdb..87b121770 100644 --- a/velodyne_driver/package.xml +++ b/velodyne_driver/package.xml @@ -23,7 +23,6 @@ dynamic_reconfigure libpcap nodelet - pluginlib roscpp tf velodyne_msgs diff --git a/velodyne_pointcloud/package.xml b/velodyne_pointcloud/package.xml index 84937b81f..3b1c3a96d 100644 --- a/velodyne_pointcloud/package.xml +++ b/velodyne_pointcloud/package.xml @@ -18,7 +18,6 @@ angles nodelet - pluginlib roscpp roslib sensor_msgs From f96c8e2d52637b1d688f2d1861b1e5f51f7501c3 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Wed, 5 Jun 2019 16:11:04 +0200 Subject: [PATCH 12/14] restore ROS code style and indentation --- .../include/velodyne_pointcloud/colors.h | 0 .../include/velodyne_pointcloud/convert.h | 19 +- .../velodyne_pointcloud/datacontainerbase.h | 290 +++++++++--------- .../organized_cloudXYZIR.h | 76 +++-- .../velodyne_pointcloud/pointcloudXYZIR.h | 31 +- .../include/velodyne_pointcloud/rawdata.h | 68 ++-- .../include/velodyne_pointcloud/transform.h | 33 +- 7 files changed, 269 insertions(+), 248 deletions(-) delete mode 100644 velodyne_pointcloud/include/velodyne_pointcloud/colors.h diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/colors.h b/velodyne_pointcloud/include/velodyne_pointcloud/colors.h deleted file mode 100644 index e69de29bb..000000000 diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h index b61b61c8a..3e04715d3 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/convert.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/convert.h @@ -39,6 +39,8 @@ #ifndef VELODYNE_POINTCLOUD_CONVERT_H #define VELODYNE_POINTCLOUD_CONVERT_H +#include + #include #include #include @@ -54,20 +56,15 @@ namespace velodyne_pointcloud class Convert { public: - Convert(ros::NodeHandle node, ros::NodeHandle private_nh); ~Convert() {} private: - - void callback(velodyne_pointcloud::CloudNodeConfig &config, - uint32_t level); + void callback(velodyne_pointcloud::CloudNodeConfig &config, uint32_t level); void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg); - ///Pointer to dynamic reconfigure service srv_ - boost::shared_ptr > srv_; - + boost::shared_ptr > srv_; + boost::shared_ptr data_; ros::Subscriber velodyne_scan_; ros::Publisher output_; @@ -77,7 +74,8 @@ class Convert boost::mutex reconfigure_mtx_; /// configuration parameters - typedef struct { + typedef struct + { std::string target_frame; ///< target frame std::string fixed_frame; ///< fixed frame bool organize_cloud; ///< enable/disable organized cloud structure @@ -85,7 +83,8 @@ class Convert double min_range; ///< minimum range to publish uint16_t num_lasers; ///< number of lasers int npackets; ///< number of packets to combine - } Config; + } + Config; Config config_; bool first_rcfg_call; diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index dea01f6e0..f231d7965 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -1,5 +1,5 @@ -#ifndef __DATACONTAINERBASE_H -#define __DATACONTAINERBASE_H +#ifndef VELODYNE_POINTCLOUD_DATACONTAINERBASE_H +#define VELODYNE_POINTCLOUD_DATACONTAINERBASE_H // Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz // All rights reserved. // @@ -36,172 +36,174 @@ #include #include #include +#include +#include #include namespace velodyne_rawdata { - class DataContainerBase +class DataContainerBase +{ +public: + DataContainerBase(const double max_range, const double min_range, const std::string& target_frame, + const std::string& fixed_frame, const unsigned int init_width, const unsigned int init_height, + const bool is_dense, const unsigned int scans_per_packet, + boost::shared_ptr& tf_ptr, int fields, ...) + : config_(max_range, min_range, target_frame, fixed_frame, init_width, init_height, is_dense, scans_per_packet) + , tf_ptr(tf_ptr) { - public: - DataContainerBase( - const double max_range, const double min_range, - const std::string& target_frame, const std::string& fixed_frame, - const unsigned int init_width, const unsigned int init_height, - const bool is_dense, const unsigned int scans_per_packet, - boost::shared_ptr& tf_ptr, - int fields, ...) - : config_(max_range, min_range, target_frame, fixed_frame, - init_width, init_height, is_dense, scans_per_packet), tf_ptr(tf_ptr) { - - va_list vl; - cloud.fields.clear(); - cloud.fields.reserve(fields); - va_start(vl, fields); - int offset = 0; - for (int i = 0; i < fields; ++i) { - // Create the corresponding PointField - std::string name(va_arg(vl, char*)); - int count(va_arg(vl, int)); - int datatype(va_arg(vl, int)); - offset = addPointField(cloud, name, count, datatype, offset); - } - va_end(vl); - cloud.point_step = offset; - cloud.row_step = cloud.width * cloud.point_step; - if(config_.transform && !tf_ptr) - { - tf_ptr = boost::shared_ptr(new tf::TransformListener); - } - + va_list vl; + cloud.fields.clear(); + cloud.fields.reserve(fields); + va_start(vl, fields); + int offset = 0; + for (int i = 0; i < fields; ++i) + { + // Create the corresponding PointField + std::string name(va_arg(vl, char*)); + int count(va_arg(vl, int)); + int datatype(va_arg(vl, int)); + offset = addPointField(cloud, name, count, datatype, offset); } - - struct Config { - double max_range; ///< maximum range to publish - double min_range; ///< minimum range to publish - std::string target_frame; ///< target frame to transform a point - std::string fixed_frame; ///< fixed frame used for transform - unsigned int init_width; - unsigned int init_height; - bool is_dense; - unsigned int scans_per_packet; - bool transform; ///< enable / disable transform points - - Config( - double max_range, double min_range, - std::string target_frame, std::string fixed_frame, - unsigned int init_width, unsigned int init_height, - bool is_dense, unsigned int scans_per_packet) - : max_range(max_range), min_range(min_range), - target_frame(target_frame), fixed_frame(fixed_frame), - transform(fixed_frame!=target_frame), - init_width(init_width), init_height(init_height), - is_dense(is_dense), scans_per_packet(scans_per_packet) - { - ROS_INFO_STREAM("Initialized container with " - << "min_range: "<< min_range << ", max_range: " << max_range - << ", target_frame: " << target_frame << ", fixed_frame: " << fixed_frame - << ", init_with: " << init_width << ", init_height: " << init_height - << ", is_dense: " << is_dense << ", scans_per_packet: " << scans_per_packet); - } - }; - - virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg){ - cloud.header = scan_msg->header; - cloud.data.resize(scan_msg->packets.size() * config_.scans_per_packet * cloud.point_step); - cloud.width = config_.init_width; - cloud.height = config_.init_height; - cloud.is_dense = static_cast(config_.is_dense); - if(config_.transform) - { - if(!computeTransformation(scan_msg->header.stamp)){ - ROS_ERROR_STREAM("Could not transform points!"); - } - } + va_end(vl); + cloud.point_step = offset; + cloud.row_step = cloud.width * cloud.point_step; + if (config_.transform && !tf_ptr) + { + tf_ptr = boost::shared_ptr(new tf::TransformListener); } + } - virtual void addPoint( - float x, float y, float z, - const uint16_t ring, const uint16_t azimuth, - const float distance, const float intensity) = 0; - virtual void newLine() = 0; - - const sensor_msgs::PointCloud2& finishCloud(){ - cloud.data.resize(cloud.point_step * cloud.width * cloud.height); - cloud.header.frame_id = config_.target_frame; - ROS_DEBUG_STREAM("Prepared cloud width" << cloud.height * cloud.width - << " Velodyne points, time: " << cloud.header.stamp); - - return cloud; + struct Config + { + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + std::string target_frame; ///< target frame to transform a point + std::string fixed_frame; ///< fixed frame used for transform + unsigned int init_width; + unsigned int init_height; + bool is_dense; + unsigned int scans_per_packet; + bool transform; ///< enable / disable transform points + + Config(double max_range, double min_range, std::string target_frame, std::string fixed_frame, + unsigned int init_width, unsigned int init_height, bool is_dense, unsigned int scans_per_packet) + : max_range(max_range) + , min_range(min_range) + , target_frame(target_frame) + , fixed_frame(fixed_frame) + , transform(fixed_frame != target_frame) + , init_width(init_width) + , init_height(init_height) + , is_dense(is_dense) + , scans_per_packet(scans_per_packet) + { + ROS_INFO_STREAM("Initialized container with " + << "min_range: " << min_range << ", max_range: " << max_range + << ", target_frame: " << target_frame << ", fixed_frame: " << fixed_frame + << ", init_with: " << init_width << ", init_height: " << init_height << ", is_dense: " << is_dense + << ", scans_per_packet: " << scans_per_packet); } + }; - void configure(const double max_range, const double min_range, - const std::string fixed_frame, const std::string target_frame) { - config_.max_range = max_range; - config_.min_range = min_range; - config_.fixed_frame = fixed_frame; - config_.target_frame = target_frame; - - config_.transform = fixed_frame.compare(target_frame) != 0; - if(config_.transform && !tf_ptr) + virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg) + { + cloud.header = scan_msg->header; + cloud.data.resize(scan_msg->packets.size() * config_.scans_per_packet * cloud.point_step); + cloud.width = config_.init_width; + cloud.height = config_.init_height; + cloud.is_dense = static_cast(config_.is_dense); + if (config_.transform) + { + if (!computeTransformation(scan_msg->header.stamp)) { - tf_ptr = boost::shared_ptr(new tf::TransformListener); + ROS_ERROR_STREAM("Could not transform points!"); } } + } - sensor_msgs::PointCloud2 cloud; + virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance, + const float intensity) = 0; + virtual void newLine() = 0; - protected: + const sensor_msgs::PointCloud2& finishCloud() + { + cloud.data.resize(cloud.point_step * cloud.width * cloud.height); + cloud.header.frame_id = config_.target_frame; + ROS_DEBUG_STREAM("Prepared cloud width" << cloud.height * cloud.width + << " Velodyne points, time: " << cloud.header.stamp); + return cloud; + } + + void configure(const double max_range, const double min_range, const std::string fixed_frame, + const std::string target_frame) + { + config_.max_range = max_range; + config_.min_range = min_range; + config_.fixed_frame = fixed_frame; + config_.target_frame = target_frame; - inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec){ - eigen_vec(0) = tf_vec[0]; - eigen_vec(1) = tf_vec[1]; - eigen_vec(2) = tf_vec[2]; + config_.transform = fixed_frame.compare(target_frame) != 0; + if (config_.transform && !tf_ptr) + { + tf_ptr = boost::shared_ptr(new tf::TransformListener); } + } - inline bool computeTransformation(const ros::Time& time){ - tf::StampedTransform transform; - try{ - tf_ptr->lookupTransform(config_.target_frame, cloud.header.frame_id, time, transform); - } - catch (tf::LookupException &e){ - ROS_ERROR ("%s", e.what ()); - return false; - } - catch (tf::ExtrapolationException &e){ - ROS_ERROR ("%s", e.what ()); - return false; - } + sensor_msgs::PointCloud2 cloud; - tf::Quaternion quaternion = transform.getRotation (); - Eigen::Quaterniond rotation ( - quaternion.w (), - quaternion.x (), - quaternion.y (), - quaternion.z () - ); - - Eigen::Vector3d eigen_origin; - vectorTfToEigen(transform.getOrigin(), eigen_origin); - Eigen::Translation3d translation ( eigen_origin ); - transformation = translation * rotation; - return true; - } +protected: + inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec) + { + eigen_vec(0) = tf_vec[0]; + eigen_vec(1) = tf_vec[1]; + eigen_vec(2) = tf_vec[2]; + } - inline void transformPoint(float& x, float& y, float& z) + inline bool computeTransformation(const ros::Time& time) + { + tf::StampedTransform transform; + try { - Eigen::Vector3d p = transformation * Eigen::Vector3d(x, y, z); - x = p.x(); y = p.y(); z = p.z(); + tf_ptr->lookupTransform(config_.target_frame, cloud.header.frame_id, time, transform); } - - inline bool pointInRange(float range) + catch (tf::LookupException& e) { - return (range >= config_.min_range - && range <= config_.max_range); + ROS_ERROR("%s", e.what()); + return false; } + catch (tf::ExtrapolationException& e) + { + ROS_ERROR("%s", e.what()); + return false; + } + + tf::Quaternion quaternion = transform.getRotation(); + Eigen::Quaterniond rotation(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); + + Eigen::Vector3d eigen_origin; + vectorTfToEigen(transform.getOrigin(), eigen_origin); + Eigen::Translation3d translation(eigen_origin); + transformation = translation * rotation; + return true; + } + + inline void transformPoint(float& x, float& y, float& z) + { + Eigen::Vector3d p = transformation * Eigen::Vector3d(x, y, z); + x = p.x(); + y = p.y(); + z = p.z(); + } + + inline bool pointInRange(float range) + { + return (range >= config_.min_range && range <= config_.max_range); + } - Config config_; - boost::shared_ptr tf_ptr; ///< transform listener - Eigen::Affine3d transformation; + Config config_; + boost::shared_ptr tf_ptr; ///< transform listener + Eigen::Affine3d transformation; }; } /* namespace velodyne_rawdata */ -#endif //__DATACONTAINERBASE_H +#endif // VELODYNE_POINTCLOUD_DATACONTAINERBASE_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h index a9b492a2b..329c98cdb 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/organized_cloudXYZIR.h @@ -1,35 +1,61 @@ -#ifndef __ORGANIZED_CLOUDXYZIR_H -#define __ORGANIZED_CLOUDXYZIR_H +// Copyright (C) 2012, 2019 Austin Robot Technology, Jack O'Quin, Joshua Whitley, Sebastian Pütz +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H +#define VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H #include #include +#include namespace velodyne_pointcloud { - class OrganizedCloudXYZIR : public velodyne_rawdata::DataContainerBase - { - public: - - OrganizedCloudXYZIR( - const double max_range, const double min_range, - const std::string& target_frame, const std::string& fixed_frame, - const unsigned int num_lasers, const unsigned int scans_per_block, - boost::shared_ptr tf_ptr = boost::shared_ptr() - ); - - virtual void newLine(); +class OrganizedCloudXYZIR : public velodyne_rawdata::DataContainerBase +{ +public: + OrganizedCloudXYZIR(const double max_range, const double min_range, const std::string& target_frame, + const std::string& fixed_frame, const unsigned int num_lasers, const unsigned int scans_per_block, + boost::shared_ptr tf_ptr = boost::shared_ptr()); - virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); + virtual void newLine(); - virtual void addPoint( - float x, float y, float z, - const uint16_t ring, const uint16_t azimuth, - const float distance, const float intensity); + virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); - private: - sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity; - sensor_msgs::PointCloud2Iterator iter_ring; - }; -} -#endif //__ORGANIZED_CLOUDXYZIR_H + virtual void addPoint(float x, float y, float z, const uint16_t ring, const uint16_t azimuth, const float distance, + const float intensity); +private: + sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity; + sensor_msgs::PointCloud2Iterator iter_ring; +}; +} /* namespace velodyne_pointcloud */ +#endif // VELODYNE_POINTCLOUD_ORGANIZED_CLOUDXYZIR_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h index 8a1d7c704..3bd0b80c0 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/pointcloudXYZIR.h @@ -34,31 +34,26 @@ #define VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H #include +#include namespace velodyne_pointcloud { - class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase - { - public: - - PointcloudXYZIR( - const double max_range, const double min_range, - const std::string& target_frame, const std::string& fixed_frame, - const unsigned int scans_per_block, - boost::shared_ptr tf_ptr = boost::shared_ptr() - ); - - virtual void newLine(); +class PointcloudXYZIR : public velodyne_rawdata::DataContainerBase +{ +public: + PointcloudXYZIR(const double max_range, const double min_range, const std::string& target_frame, + const std::string& fixed_frame, const unsigned int scans_per_block, + boost::shared_ptr tf_ptr = boost::shared_ptr()); - virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); + virtual void newLine(); - virtual void addPoint(float x, float y, float z, - uint16_t ring, uint16_t azimuth, float distance, float intensity); + virtual void setup(const velodyne_msgs::VelodyneScan::ConstPtr& scan_msg); - sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity; - sensor_msgs::PointCloud2Iterator iter_ring; + virtual void addPoint(float x, float y, float z, uint16_t ring, uint16_t azimuth, float distance, float intensity); - }; + sensor_msgs::PointCloud2Iterator iter_x, iter_y, iter_z, iter_intensity; + sensor_msgs::PointCloud2Iterator iter_ring; +}; } // namespace velodyne_pointcloud #endif // VELODYNE_POINTCLOUD_POINTCLOUDXYZIR_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h index 9dfa3b23c..ed0095993 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h @@ -63,21 +63,19 @@ static const int RAW_SCAN_SIZE = 3; static const int SCANS_PER_BLOCK = 32; static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); -static const float ROTATION_RESOLUTION = 0.01f; // [deg] -static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100] +static const float ROTATION_RESOLUTION = 0.01f; // [deg] +static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100] /** @todo make this work for both big and little-endian machines */ static const uint16_t UPPER_BANK = 0xeeff; static const uint16_t LOWER_BANK = 0xddff; - /** Special Defines for VLP16 support **/ -static const int VLP16_FIRINGS_PER_BLOCK = 2; -static const int VLP16_SCANS_PER_FIRING = 16; -static const float VLP16_BLOCK_TDURATION = 110.592f; // [µs] -static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] -static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] - +static const int VLP16_FIRINGS_PER_BLOCK = 2; +static const int VLP16_SCANS_PER_FIRING = 16; +static const float VLP16_BLOCK_TDURATION = 110.592f; // [µs] +static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] +static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] /** \brief Raw Velodyne data block. * @@ -88,9 +86,9 @@ static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] */ typedef struct raw_block { - uint16_t header; ///< UPPER_BANK or LOWER_BANK - uint16_t rotation; ///< 0-35999, divide by 100 to get degrees - uint8_t data[BLOCK_DATA_SIZE]; + uint16_t header; ///< UPPER_BANK or LOWER_BANK + uint16_t rotation; ///< 0-35999, divide by 100 to get degrees + uint8_t data[BLOCK_DATA_SIZE]; } raw_block_t; @@ -102,7 +100,7 @@ raw_block_t; union two_bytes { uint16_t uint; - uint8_t bytes[2]; + uint8_t bytes[2]; }; static const int PACKET_SIZE = 1206; @@ -135,7 +133,9 @@ class RawData { public: RawData(); - ~RawData() {} + ~RawData() + { + } /** \brief Set up for data processing. * @@ -150,22 +150,21 @@ class RawData boost::optional setup(ros::NodeHandle private_nh); /** \brief Set up for data processing offline. - * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle. - * this method is useful if unpacking data directly from bag files, without passing - * through a communication overhead. - * - * @param calibration_file path to the calibration file - * @param max_range_ cutoff for maximum range - * @param min_range_ cutoff for minimum range - * @returns 0 if successful; - * errno value for failure - */ + * Performs the same initialization as in setup, in the abscence of a ros::NodeHandle. + * this method is useful if unpacking data directly from bag files, without passing + * through a communication overhead. + * + * @param calibration_file path to the calibration file + * @param max_range_ cutoff for maximum range + * @param min_range_ cutoff for minimum range + * @returns 0 if successful; + * errno value for failure + */ int setupOffline(std::string calibration_file, double max_range_, double min_range_); - void unpack(const velodyne_msgs::VelodynePacket &pkt, DataContainerBase& data); + void unpack(const velodyne_msgs::VelodynePacket& pkt, DataContainerBase& data); - void setParameters(double min_range, double max_range, double view_direction, - double view_width); + void setParameters(double min_range, double max_range, double view_direction, double view_width); int scansPerPacket() const; @@ -173,11 +172,11 @@ class RawData /** configuration parameters */ typedef struct { - std::string calibrationFile; ///< calibration file name - double max_range; ///< maximum range to publish - double min_range; ///< minimum range to publish - int min_angle; ///< minimum angle to publish - int max_angle; ///< maximum angle to publish + std::string calibrationFile; ///< calibration file name + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + int min_angle; ///< minimum angle to publish + int max_angle; ///< maximum angle to publish double tmp_min_angle; double tmp_max_angle; @@ -193,10 +192,9 @@ class RawData float cos_rot_table_[ROTATION_MAX_UNITS]; /** add private function to handle the VLP16 **/ - void unpack_vlp16(const velodyne_msgs::VelodynePacket &pkt, DataContainerBase& data); - + void unpack_vlp16(const velodyne_msgs::VelodynePacket& pkt, DataContainerBase& data); }; -} // namespace velodyne_rawdata +} // namespace velodyne_rawdata #endif // VELODYNE_POINTCLOUD_RAWDATA_H diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h index 894eda605..5291fcdea 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/transform.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/transform.h @@ -1,5 +1,5 @@ -// Copyright (C) 2009, 2010, 2011, 2012, 2019 Austin Robot Technology, Jack O'Quin, Jesse Vera, Joshua Whitley, Sebastian Pütz -// All rights reserved. +// Copyright (C) 2009, 2010, 2011, 2012, 2019 Austin Robot Technology, Jack O'Quin, Jesse Vera, Joshua Whitley, +// Sebastian Pütz All rights reserved. // // Software License Agreement (BSD License 2.0) // @@ -40,6 +40,7 @@ #ifndef VELODYNE_POINTCLOUD_TRANSFORM_H #define VELODYNE_POINTCLOUD_TRANSFORM_H +#include #include #include "tf/message_filter.h" #include "message_filters/subscriber.h" @@ -61,17 +62,16 @@ class Transform { public: Transform(ros::NodeHandle node, ros::NodeHandle private_nh); - ~Transform() {} + ~Transform() + { + } private: - void processScan(const velodyne_msgs::VelodyneScan::ConstPtr &scanMsg); + void processScan(const velodyne_msgs::VelodyneScan::ConstPtr& scanMsg); // Pointer to dynamic reconfigure service srv_ - boost::shared_ptr> srv_; - void reconfigure_callback( - velodyne_pointcloud::TransformNodeConfig &config, - uint32_t level); + boost::shared_ptr> srv_; + void reconfigure_callback(velodyne_pointcloud::TransformNodeConfig& config, uint32_t level); const std::string tf_prefix_; boost::shared_ptr data_; @@ -81,13 +81,14 @@ class Transform boost::shared_ptr tf_ptr_; /// configuration parameters - typedef struct { - std::string target_frame; ///< target frame - std::string fixed_frame; ///< fixed frame - bool organize_cloud; ///< enable/disable organized cloud structure - double max_range; ///< maximum range to publish - double min_range; ///< minimum range to publish - uint16_t num_lasers; ///< number of lasers + typedef struct + { + std::string target_frame; ///< target frame + std::string fixed_frame; ///< fixed frame + bool organize_cloud; ///< enable/disable organized cloud structure + double max_range; ///< maximum range to publish + double min_range; ///< minimum range to publish + uint16_t num_lasers; ///< number of lasers } Config; Config config_; From b31561e169b7f3b4e26e69b9b66c2d9674998a37 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Thu, 6 Jun 2019 14:32:11 +0200 Subject: [PATCH 13/14] Change eigen3 include dirs cmake variable for backwards compatibility --- velodyne_pointcloud/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/velodyne_pointcloud/CMakeLists.txt b/velodyne_pointcloud/CMakeLists.txt index 2fdb4d92a..d45b9f040 100644 --- a/velodyne_pointcloud/CMakeLists.txt +++ b/velodyne_pointcloud/CMakeLists.txt @@ -52,7 +52,7 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") include_directories(include ${catkin_INCLUDE_DIRS} ${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake - ${EIGEN3_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} ) catkin_package( From 502867f2937b609c190686b1b927d095f2dd4534 Mon Sep 17 00:00:00 2001 From: Sebastian Date: Thu, 6 Jun 2019 15:16:20 +0200 Subject: [PATCH 14/14] Eigen3 precision change from double to float --- .../include/velodyne_pointcloud/datacontainerbase.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h index f231d7965..cb181a97a 100644 --- a/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h +++ b/velodyne_pointcloud/include/velodyne_pointcloud/datacontainerbase.h @@ -153,7 +153,7 @@ class DataContainerBase sensor_msgs::PointCloud2 cloud; protected: - inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3d& eigen_vec) + inline void vectorTfToEigen(tf::Vector3& tf_vec, Eigen::Vector3f& eigen_vec) { eigen_vec(0) = tf_vec[0]; eigen_vec(1) = tf_vec[1]; @@ -179,18 +179,18 @@ class DataContainerBase } tf::Quaternion quaternion = transform.getRotation(); - Eigen::Quaterniond rotation(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); + Eigen::Quaternionf rotation(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()); - Eigen::Vector3d eigen_origin; + Eigen::Vector3f eigen_origin; vectorTfToEigen(transform.getOrigin(), eigen_origin); - Eigen::Translation3d translation(eigen_origin); + Eigen::Translation3f translation(eigen_origin); transformation = translation * rotation; return true; } inline void transformPoint(float& x, float& y, float& z) { - Eigen::Vector3d p = transformation * Eigen::Vector3d(x, y, z); + Eigen::Vector3f p = transformation * Eigen::Vector3f(x, y, z); x = p.x(); y = p.y(); z = p.z(); @@ -203,7 +203,7 @@ class DataContainerBase Config config_; boost::shared_ptr tf_ptr; ///< transform listener - Eigen::Affine3d transformation; + Eigen::Affine3f transformation; }; } /* namespace velodyne_rawdata */ #endif // VELODYNE_POINTCLOUD_DATACONTAINERBASE_H