diff --git a/modules/drivers/lidar_velodyne/pointcloud/BUILD b/modules/drivers/lidar_velodyne/pointcloud/BUILD index 24584a0c1db..3a1b50db8fa 100644 --- a/modules/drivers/lidar_velodyne/pointcloud/BUILD +++ b/modules/drivers/lidar_velodyne/pointcloud/BUILD @@ -20,4 +20,24 @@ cc_library( ], ) +cc_library( + name = "convert", + srcs = [ + "convert.cc", + "convert_nodelet.cc", + ], + hdrs = [ + "convert.h", + ], + deps = [ + "//modules/common", + "//modules/drivers/lidar_velodyne/pointcloud/lib:util", + "//modules/drivers/lidar_velodyne/pointcloud/lib:velodyne_parser", + "@ros//:ros_common", + "@eigen", + "@pcl", + ], +) + + cpplint() diff --git a/modules/drivers/lidar_velodyne/pointcloud/compensator.cc b/modules/drivers/lidar_velodyne/pointcloud/compensator.cc index 5cde71c38cd..b8867a2adf5 100644 --- a/modules/drivers/lidar_velodyne/pointcloud/compensator.cc +++ b/modules/drivers/lidar_velodyne/pointcloud/compensator.cc @@ -16,6 +16,9 @@ #include "modules/drivers/lidar_velodyne/pointcloud/compensator.h" +#include +#include + #include "ros/this_node.h" namespace apollo { @@ -35,14 +38,14 @@ Compensator::Compensator(ros::NodeHandle& node, ros::NodeHandle& private_nh) topic_compensated_pointcloud_, TOPIC_COMPENSATED_POINTCLOUD); private_nh.param("topic_pointcloud", topic_pointcloud_, TOPIC_POINTCLOUD); private_nh.param("queue_size", queue_size_, 10); - private_nh.param("tf_query_timeout", tf_timeout_, float(0.1)); + private_nh.param("tf_query_timeout", tf_timeout_, static_cast(0.1)); // advertise output point cloud (before subscribing to input data) compensation_pub_ = node.advertise( topic_compensated_pointcloud_, queue_size_); - pointcloud_sub_ = - node.subscribe(topic_pointcloud_, queue_size_, - &Compensator::pointcloud_callback, (Compensator*)this); + pointcloud_sub_ = node.subscribe(topic_pointcloud_, queue_size_, + &Compensator::pointcloud_callback, + reinterpret_cast(this)); } void Compensator::pointcloud_callback(sensor_msgs::PointCloud2ConstPtr msg) { @@ -59,8 +62,8 @@ void Compensator::pointcloud_callback(sensor_msgs::PointCloud2ConstPtr msg) { get_timestamp_interval(msg, timestamp_min, timestamp_max); // compensate point cloud, remove nan point - if (query_pose_affine_from_tf2(timestamp_min, pose_min_time) && - query_pose_affine_from_tf2(timestamp_max, pose_max_time)) { + if (query_pose_affine_from_tf2(timestamp_min, &pose_min_time) && + query_pose_affine_from_tf2(timestamp_max, &pose_max_time)) { // we change message after motion compensation sensor_msgs::PointCloud2::Ptr q_msg(new sensor_msgs::PointCloud2()); *q_msg = *msg; @@ -93,7 +96,7 @@ inline void Compensator::get_timestamp_interval( } } -// TODO: if point type is always float, and timestamp is always double? +// TODO(All): if point type is always float, and timestamp is always double? inline bool Compensator::check_message(sensor_msgs::PointCloud2ConstPtr msg) { // check msg width and height if (msg->width == 0 || msg->height == 0) { @@ -104,7 +107,7 @@ inline bool Compensator::check_message(sensor_msgs::PointCloud2ConstPtr msg) { int y_data_type = 0; int z_data_type = 0; - // TODO: will use a new datastruct with interface to get offset, + // TODO(All): will use a new datastruct with interface to get offset, // datatype,datasize... for (size_t i = 0; i < msg->fields.size(); ++i) { const sensor_msgs::PointField& f = msg->fields[i]; @@ -152,7 +155,10 @@ inline bool Compensator::check_message(sensor_msgs::PointCloud2ConstPtr msg) { } bool Compensator::query_pose_affine_from_tf2(const double timestamp, - Eigen::Affine3d& pose) { + Eigen::Affine3d* pose) { + if (pose == nullptr) { + return false; + } ros::Time query_time(timestamp); std::string err_string; if (!tf2_buffer_.canTransform("world", child_frame_id_, query_time, @@ -173,8 +179,7 @@ bool Compensator::query_pose_affine_from_tf2(const double timestamp, return false; } - tf::transformMsgToEigen(stamped_transform.transform, pose); - // ROS_DEBUG_STREAM("pose matrix : " << pose); + tf::transformMsgToEigen(stamped_transform.transform, *pose); return true; } diff --git a/modules/drivers/lidar_velodyne/pointcloud/compensator.h b/modules/drivers/lidar_velodyne/pointcloud/compensator.h index f134623ecf5..6548d1703c5 100644 --- a/modules/drivers/lidar_velodyne/pointcloud/compensator.h +++ b/modules/drivers/lidar_velodyne/pointcloud/compensator.h @@ -17,7 +17,7 @@ #ifndef MODULES_DRIVERS_LIDAR_VELODYNE_POINTCLOUD_COMPENSATOR_H_ #define MODULES_DRIVERS_LIDAR_VELODYNE_POINTCLOUD_COMPENSATOR_H_ -#include "modules/drivers/lidar_velodyne/pointcloud/lib/const_variables.h" +#include #include "Eigen/Eigen" #include "eigen_conversions/eigen_msg.h" @@ -27,13 +27,15 @@ #include "std_msgs/String.h" #include "tf2_ros/transform_listener.h" +#include "modules/drivers/lidar_velodyne/pointcloud/lib/const_variables.h" + namespace apollo { namespace drivers { namespace lidar_velodyne { class Compensator { public: - Compensator(ros::NodeHandle& node, ros::NodeHandle& private_nh); + Compensator(ros::NodeHandle& node, ros::NodeHandle& private_nh); // NOLINT virtual ~Compensator() = default; private: @@ -47,7 +49,7 @@ class Compensator { * novatel-preprocess broadcast the tf2 transfrom. */ bool query_pose_affine_from_tf2(const double timestamp, - Eigen::Affine3d& pose); + Eigen::Affine3d* pose); /** * @brief check if message is valid, check width, height, timestamp. * set timestamp_offset and point data type @@ -57,7 +59,7 @@ class Compensator { * @brief motion compensation for point cloud */ template - void motion_compensation(sensor_msgs::PointCloud2::Ptr& msg, + void motion_compensation(sensor_msgs::PointCloud2::Ptr& msg, // NOLINT const double timestamp_min, const double timestamp_max, const Eigen::Affine3d& pose_min_time, @@ -66,8 +68,9 @@ class Compensator { * @brief get min timestamp and max timestamp from points in pointcloud2 */ inline void get_timestamp_interval(sensor_msgs::PointCloud2ConstPtr msg, - double& timestamp_min, - double& timestamp_max); + double& timestamp_min, // NOLINT + double& timestamp_max); // NOLINT + /** * @brief get point field size by sensor_msgs::datatype */ diff --git a/modules/drivers/lidar_velodyne/pointcloud/convert.cc b/modules/drivers/lidar_velodyne/pointcloud/convert.cc new file mode 100644 index 00000000000..18583c69d59 --- /dev/null +++ b/modules/drivers/lidar_velodyne/pointcloud/convert.cc @@ -0,0 +1,91 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "modules/drivers/lidar_velodyne/pointcloud/convert.h" + +#include "pcl/common/time.h" +#include "pcl_conversions/pcl_conversions.h" +#include "ros/advertise_options.h" + +namespace apollo { +namespace drivers { +namespace lidar_velodyne { + +void Convert::init(ros::NodeHandle& node, ros::NodeHandle& private_nh) { + private_nh.param("max_range", config_.max_range, 130.0); + private_nh.param("min_range", config_.min_range, 0.9); + private_nh.param("view_direction", config_.view_direction, 0.0); + private_nh.param("view_width", config_.view_width, 2.0 * M_PI); + private_nh.param("model", config_.model, std::string("64E_S2")); + private_nh.param("calibration_online", config_.calibration_online, true); + private_nh.param("calibration", config_.calibration_file, std::string("")); + private_nh.param("organized", config_.organized, false); + private_nh.param("topic_packets", topic_packets_, TOPIC_PACKTES); + private_nh.param("topic_pointcloud", topic_pointcloud_, TOPIC_POINTCLOUD); + // we use beijing time by default + private_nh.param("queue_size", queue_size_, 10); + + parser_.reset(VelodyneParserFactory::create_parser(config_)); + if (parser_.get() == nullptr) { + ROS_BREAK(); + } + parser_->setup(); + // Emphasis no header available in published msg, which enables us to + // customize header.seq. + // Learn from + // http://answers.ros.org/question/55126/why-does-ros-overwrite-my-sequence-number/ + // ros::AdvertiseOptions opt = + // ros::AdvertiseOptions::create( + // topic_pointcloud_, queue_size_, &connected, &disconnected, + // ros::VoidPtr(), NULL); + // opt.has_header = false; + // velodyne_points_output_ = node.advertise(opt); + pointcloud_pub_ = + node.advertise(topic_pointcloud_, queue_size_); + + // subscribe to VelodyneScan packets + velodyne_scan_ = node.subscribe( + topic_packets_, queue_size_, &Convert::convert_packets_to_pointcloud, + reinterpret_cast(this), ros::TransportHints().tcpNoDelay(true)); +} + +/** @brief Callback for raw scan messages. */ +void Convert::convert_packets_to_pointcloud( + velodyne_msgs::VelodyneScanUnified::ConstPtr scan_msg) { + ROS_INFO_ONCE("********************************************************"); + ROS_INFO_ONCE("Start convert velodyne packets to pointcloud"); + ROS_INFO_ONCE("********************************************************"); + ROS_DEBUG_STREAM(scan_msg->header.seq); + + VPointCloud::Ptr pointcloud(new VPointCloud()); + parser_->generate_pointcloud(scan_msg, pointcloud); + + if (pointcloud->empty()) { + return; + } + + if (config_.organized) { + ROS_DEBUG_STREAM("reorder point cloud"); + parser_->order(pointcloud); + } + + // publish the accumulated cloud message + pointcloud_pub_.publish(pointcloud); +} + +} // namespace lidar_velodyne +} // namespace drivers +} // namespace apollo diff --git a/modules/drivers/lidar_velodyne/pointcloud/convert.h b/modules/drivers/lidar_velodyne/pointcloud/convert.h new file mode 100644 index 00000000000..a53db424e74 --- /dev/null +++ b/modules/drivers/lidar_velodyne/pointcloud/convert.h @@ -0,0 +1,68 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#ifndef MODULES_DRIVERS_LIDAR_VELODYNE_POINTCLOUD_CONVERT_H_ +#define MODULES_DRIVERS_LIDAR_VELODYNE_POINTCLOUD_CONVERT_H_ + +#include +#include + +#include "nav_msgs/Odometry.h" +#include "ros/ros.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/PointCloud2.h" +#include "std_msgs/Time.h" + +#include "modules/drivers/lidar_velodyne/pointcloud/lib/velodyne_parser.h" + +namespace apollo { +namespace drivers { +namespace lidar_velodyne { + +// convert velodyne data to pointcloud and republish +class Convert { + public: + Convert() = default; + virtual ~Convert() = default; + + // init velodyne config struct from private_nh + void init(ros::NodeHandle& node, ros::NodeHandle& private_nh); // NOLINT + + private: + // convert velodyne data to pointcloud and public + void convert_packets_to_pointcloud( + velodyne_msgs::VelodyneScanUnified::ConstPtr scan_msg); + + // RawData class for converting data to point cloud + std::unique_ptr parser_; + + ros::Subscriber velodyne_scan_; + ros::Publisher pointcloud_pub_; + + std::string topic_packets_; + std::string topic_pointcloud_; + + /// configuration parameters, get config struct from velodyne_parser.h + Config config_; + // queue size for ros node pub + int queue_size_; +}; + +} // namespace lidar_velodyne +} // namespace drivers +} // namespace apollo + +#endif // MODULES_DRIVERS_LIDAR_VELODYNE_POINTCLOUD_CONVERT_H_ diff --git a/modules/drivers/lidar_velodyne/pointcloud/convert_nodelet.cc b/modules/drivers/lidar_velodyne/pointcloud/convert_nodelet.cc new file mode 100644 index 00000000000..d1857bf35b2 --- /dev/null +++ b/modules/drivers/lidar_velodyne/pointcloud/convert_nodelet.cc @@ -0,0 +1,53 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "nodelet/nodelet.h" +#include "pluginlib/class_list_macros.h" +#include "ros/ros.h" + +#include "modules/drivers/lidar_velodyne/pointcloud/convert.h" + +namespace apollo { +namespace drivers { +namespace lidar_velodyne { + +class ConvertNodelet : public nodelet::Nodelet { + public: + ConvertNodelet() {} + ~ConvertNodelet() {} + + private: + virtual void onInit(); + boost::shared_ptr conv_; +}; + +/** @brief Nodelet initialization. */ +void ConvertNodelet::onInit() { + ROS_INFO("Point cloud nodelet init"); + conv_.reset(new Convert()); + conv_->init(getNodeHandle(), getPrivateNodeHandle()); +} + +} // namespace lidar_velodyne +} // namespace drivers +} // namespace apollo + +// Register this plugin with pluginlib. Names must match nodelet_velodyne.xml. +// +// parameters: package, class name, class type, base class type +PLUGINLIB_DECLARE_CLASS(velodyne_pointcloud, ConvertNodelet, + apollo::drivers::lidar_velodyne::ConvertNodelet, + nodelet::Nodelet);