forked from ApolloAuto/apollo
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Bazel build velodyne driver: added convert
- Loading branch information
1 parent
95bf00c
commit 8d0d74f
Showing
6 changed files
with
257 additions
and
17 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<sensor_msgs::PointCloud2>( | ||
| // topic_pointcloud_, queue_size_, &connected, &disconnected, | ||
| // ros::VoidPtr(), NULL); | ||
| // opt.has_header = false; | ||
| // velodyne_points_output_ = node.advertise(opt); | ||
| pointcloud_pub_ = | ||
| node.advertise<sensor_msgs::PointCloud2>(topic_pointcloud_, queue_size_); | ||
|
|
||
| // subscribe to VelodyneScan packets | ||
| velodyne_scan_ = node.subscribe( | ||
| topic_packets_, queue_size_, &Convert::convert_packets_to_pointcloud, | ||
| reinterpret_cast<Convert*>(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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <memory> | ||
| #include <string> | ||
|
|
||
| #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<VelodyneParser> 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_ |
53 changes: 53 additions & 0 deletions
53
modules/drivers/lidar_velodyne/pointcloud/convert_nodelet.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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<Convert> 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); |