diff --git a/CMakeLists.txt b/CMakeLists.txt index ae36340..ec35889 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,102 +1,114 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(laser_assembler) -############################################################################## -# Find dependencies -############################################################################## - -set(THIS_PACKAGE_ROS_DEPS -tf sensor_msgs message_filters roscpp laser_geometry filters) - -find_package(catkin REQUIRED COMPONENTS - ${THIS_PACKAGE_ROS_DEPS} - message_generation) -find_package(Boost REQUIRED COMPONENTS system signals) -include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - -############################################################################## -# Build service definitions -############################################################################## -add_service_files(FILES -AssembleScans.srv -AssembleScans2.srv) - -generate_messages(DEPENDENCIES sensor_msgs std_msgs) - -############################################################################## -# Define package -############################################################################## - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS ${THIS_PACKAGE_ROS_DEPS} message_runtime - DEPENDS - ) - -############################################################################## -# Build -############################################################################## +if(NOT WIN32) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") +endif() -add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp) -target_link_libraries(laser_scan_assembler_srv ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(laser_scan_assembler_srv ${PROJECT_NAME}_gencpp) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rosidl_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(laser_geometry REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2 REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(filters REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} "srv/AssembleScans.srv" + "srv/AssembleScans2.srv" + DEPENDENCIES builtin_interfaces sensor_msgs) + +set(INCLUDE_DIRS include ${ament_cmake_INCLUDE_DIRS} + ${rosidl_default_generators_INCLUDE_DIRS} ${signals_INCLUDE_DIRS} + ${builtin_interfaces_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} ${std_msgs_INCLUDE_DIRS} + ${laser_geometry_INCLUDE_DIRS} + ${tf2_ros_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} + ${message_filters_INCLUDE_DIRS} + ${rostest_INCLUDE_DIRS} ${system_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${filters_INCLUDE_DIRS}) + +include_directories(${INCLUDE_DIRS}) + +set(LIBS ${rclcpp_LIBRARIES} ${ament_cmake_LIBRARIES} + ${rosidl_default_generators_LIBRARIES} ${signals_LIBRARIES} + ${std_msgs_LIBRARIES} + ${system_LIBRARIES} ${tf2_ros_LIBRARIES} ${tf2_LIBRARIES} + ${laser_geometry_LIBRARIES} + ${geometry_msgs_LIBRARIES} + ${message_filters_LIBRARIES} ${sensor_msgs_LIBRARIES} + ${filters_LIBRARIES}) add_executable(laser_scan_assembler src/laser_scan_assembler.cpp) -target_link_libraries(laser_scan_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(laser_scan_assembler ${PROJECT_NAME}_gencpp) - -add_executable(merge_clouds src/merge_clouds.cpp) -target_link_libraries(merge_clouds ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(merge_clouds ${PROJECT_NAME}_gencpp) - -add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp) -target_link_libraries(point_cloud_assembler_srv ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(point_cloud_assembler_srv ${PROJECT_NAME}_gencpp) - -add_executable(point_cloud_assembler src/point_cloud_assembler.cpp) -target_link_libraries(point_cloud_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(point_cloud_assembler ${PROJECT_NAME}_gencpp) - -add_executable(point_cloud2_assembler src/point_cloud2_assembler.cpp) -target_link_libraries(point_cloud2_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(point_cloud2_assembler ${PROJECT_NAME}_gencpp) - -## unit testing +target_link_libraries(laser_scan_assembler ${LIBS}) +rosidl_target_interfaces(laser_scan_assembler + ${PROJECT_NAME} "rosidl_typesupport_cpp") +install( + TARGETS laser_scan_assembler + DESTINATION lib/${PROJECT_NAME}) + +include_directories(include) + +install(DIRECTORY "include/" + DESTINATION include) + +install(TARGETS laser_scan_assembler + DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include) + +ament_export_include_directories(include) +ament_export_dependencies(rclcpp) +ament_export_dependencies(signals) +ament_export_dependencies(sensor_msgs) +ament_export_dependencies(tf2_ros) +ament_export_dependencies(tf2) +ament_export_dependencies(message_filters) +ament_export_dependencies(laser_geometry) +ament_export_include_directories(${INCLUDE_DIRS}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) -if(CATKIN_ENABLE_TESTING) - find_package(rostest) - - add_executable(periodic_snapshotter examples/periodic_snapshotter.cpp) - target_link_libraries(periodic_snapshotter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - add_dependencies(periodic_snapshotter ${PROJECT_NAME}_gencpp) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() add_executable(dummy_scan_producer test/dummy_scan_producer.cpp) - target_link_libraries(dummy_scan_producer ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - add_dependencies(dummy_scan_producer ${PROJECT_NAME}_gencpp) + target_link_libraries(dummy_scan_producer ${LIBS}) + rosidl_target_interfaces(dummy_scan_producer + ${PROJECT_NAME} "rosidl_typesupport_cpp") + install( + TARGETS dummy_scan_producer + DESTINATION lib/${PROJECT_NAME}) + + ament_add_gtest(test_assembler test/test_assembler.cpp) + target_link_libraries(test_assembler ${LIBS}) + rosidl_target_interfaces(test_assembler + ${PROJECT_NAME} "rosidl_typesupport_cpp") + install( + TARGETS test_assembler + DESTINATION lib/${PROJECT_NAME}) - add_executable(test_assembler test/test_assembler.cpp) - target_link_libraries(test_assembler ${catkin_LIBRARIES} ${Boost_LIBRARIES} gtest) - add_dependencies(test_assembler ${PROJECT_NAME}_gencpp) + add_executable(periodic_snapshotter examples/periodic_snapshotter.cpp) + target_link_libraries(periodic_snapshotter ${LIBS}) + rosidl_target_interfaces(periodic_snapshotter + ${PROJECT_NAME} "rosidl_typesupport_cpp") + install( + TARGETS periodic_snapshotter + DESTINATION lib/${PROJECT_NAME}) - add_rostest(test/test_laser_assembler.launch) endif() -############################################################################## -# Install -############################################################################## - -install(TARGETS - laser_scan_assembler_srv - laser_scan_assembler - merge_clouds - point_cloud_assembler_srv - point_cloud_assembler - point_cloud2_assembler - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Install headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) +ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..6f63de9 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,13 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..987ef96 --- /dev/null +++ b/README.md @@ -0,0 +1,92 @@ +This file describes the work done and steps to perform test on the laser_assembler package with bouncy release. + +"https://github.com/vandanamandlik/laser_assembler/tree/ros2-devel" + + +ROS2 Migration changes + + The basic concept and design are same as ROS1. + Work Done by referring ROS1 hydro-devel branch of laser_assembler package. + All changes for migration have been done as per Migration guide. + Migrated all header and source files into ROS2 style + Migrated yaml files into ROS2 style + Migrated .launch file into launch.py in ROS2 style + Migrated CMakeLists.txt and package.xml in ROS2 style + +Dependencies + + filters + laser_geometry + message_filters + launch + +Build packages + + mkdir test_laser_assembler + + Build filters package + Go to test_laser_assembler directory + mkdir filters_ws + mkdir filters_ws/src + cd filters_ws/src + git clone https://github.com/swatifulzele/filters.git -b ros2_devel + Go to filters_ws directory. + source /opt/ros/bouncy/setup.bash + colcon build + + Build laser_geometry package + Go to test_laser_assembler directory + mkdir laser_geometry_ws + mkdir laser_geometry_ws/src + cd laser_geometry_ws/src + git clone https://github.com/vandanamandlik/laser_geometry.git -b ros2-devel(laser_geometry package was not having PointCloud1 support so I have added it here.) + Go to laser_geometry_ws directory. + colcon build + + Build message_filters package + Go to test_laser_assembler directory + mkdir message_filters_ws + mkdir message_filters_ws/src + cd message_filters_ws/src + git clone https://github.com/vandanamandlik/ros2_message_filters.git -b ros2-devel(Fixed error: FrameId is not a member of message_filters::message_traits) + Go to message_filters_ws directory. + colcon build + + Build launch package + Go to test_laser_assembler directory + mkdir launch_ws + mkdir launch_ws/src + cd launch_ws/src + git clone https://github.com/ros2/launch.git -b master + Go to launch_ws directory. + colcon build + + Build laser_assembler + Go to test_laser_assembler directory + mkdir laser_assembler_ws + mkdir laser_assembler_ws/src + git clone https://github.com/vandanamandlik/laser_assembler.git -b ros2-devel + Go to laser_assembler_ws + source (eg. source filters_ws/install/setup.sh) + source + source + colcon build + +Do the test + + Here launch files are used independently. + Following are the steps to run the test cases independently: + + 1. Set the path + Go to laser_assembler_ws + source /opt/ros/bouncy/setup.bash + source ./install/setup.bash + source (source ../launch_ws/install/setup.bash) + + 2. To run non_zero_point_cloud_test + ros2 launch laser_assembler test_laser_assembler.launch.py + +Limitations + + colcon test does not work as launch.py files can not be executed/added with CMakeLists.txt as of now. + diff --git a/examples/periodic_snapshotter.cpp b/examples/periodic_snapshotter.cpp index 0c6ef17..5dccf2e 100644 --- a/examples/periodic_snapshotter.cpp +++ b/examples/periodic_snapshotter.cpp @@ -1,48 +1,33 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 #include -#include - +#include +#include // Services -#include "laser_assembler/AssembleScans.h" +#include "laser_assembler/srv/assemble_scans.hpp" +#include "rclcpp/rclcpp.hpp" // Messages -#include "sensor_msgs/PointCloud.h" +#include "rclcpp/clock.hpp" +#include "rcutils/cmdline_parser.h" +#include "sensor_msgs/msg/point_cloud.hpp" +#include "std_msgs/msg/string.hpp" - -/*** +/** * This a simple test app that requests a point cloud from the * point_cloud_assembler every 4 seconds, and then publishes the * resulting data @@ -50,74 +35,98 @@ namespace laser_assembler { -class PeriodicSnapshotter +class PeriodicSnapshotter : public rclcpp::Node { - public: - - PeriodicSnapshotter() + explicit PeriodicSnapshotter(const std::string & service_name) + : Node(service_name) { - // Create a publisher for the clouds that we assemble - pub_ = n_.advertise ("assembled_cloud", 1); + static const char SERVICE_NAME[] = "assemble_scans"; + client_ = this->create_client( + SERVICE_NAME); + + using namespace std::chrono_literals; + if (!client_->wait_for_service(20s)) { + RCLCPP_INFO(this->get_logger(), "Service not available after waiting"); + return; + } + RCLCPP_INFO(this->get_logger(), "Service [%s] detected", SERVICE_NAME); - // Create the service client for calling the assembler - client_ = n_.serviceClient("assemble_scans"); + // Create a function for when messages are to be sent. + auto timer_callback = [this]() -> void { + this->timerCallback(); + }; - // Start the timer that will trigger the processing loop (timerCallback) - timer_ = n_.createTimer(ros::Duration(5,0), &PeriodicSnapshotter::timerCallback, this); + // Create a publisher with a custom Quality of Service profile. + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default; + custom_qos_profile.depth = 7; + pub_ = this->create_publisher( + "/assembled_cloud", custom_qos_profile); + + // Use a timer to schedule periodic message publishing. + timer_ = + this->create_wall_timer(std::chrono::milliseconds(25), timer_callback); // Need to track if we've called the timerCallback at least once first_time_ = true; } - void timerCallback(const ros::TimerEvent& e) + // void timerCallback(const builtin_interfaces::msg::Time& e) + void timerCallback() { - // We don't want to build a cloud the first callback, since we we // don't have a start and end time yet - if (first_time_) - { + if (first_time_) { + last_time = rclcpp::Clock().now(); first_time_ = false; return; } // Populate our service request based on our timer callback times - AssembleScans srv; - srv.request.begin = e.last_real; - srv.request.end = e.current_real; - - // Make the service call - if (client_.call(srv)) - { - ROS_INFO("Published Cloud with %u points", (uint32_t)(srv.response.cloud.points.size())) ; - pub_.publish(srv.response.cloud); - } - else - { - ROS_ERROR("Error making service call\n") ; - } + auto request = std::make_shared< + laser_assembler::srv::AssembleScans::Request>(); + + request->begin = last_time.sec; + last_time = rclcpp::Clock().now(); + request->end = last_time.sec; + + using ServiceResponseFuture = rclcpp::Client< + laser_assembler::srv::AssembleScans>::SharedFuture; + + auto response_received_callback = [this](ServiceResponseFuture future) { + auto result = future.get(); + if (result.get()->cloud.points.size() > 0) { + RCLCPP_INFO(this->get_logger(), "Got result: [ %ld ]", result.get()->cloud.points.size()); + pub_->publish(result.get()->cloud); + } else { + RCLCPP_INFO(this->get_logger(), + "Got an empty cloud. Going to try again on the next scan"); + } + }; + + auto result = client_->async_send_request(request); } private: - ros::NodeHandle n_; - ros::Publisher pub_; - ros::ServiceClient client_; - ros::Timer timer_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Client::SharedPtr + client_; + builtin_interfaces::msg::Time last_time; + rclcpp::TimerBase::SharedPtr timer_; bool first_time_; -} ; +}; -} - -using namespace laser_assembler ; +} // namespace laser_assembler -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "periodic_snapshotter"); - ros::NodeHandle n; - ROS_INFO("Waiting for [build_cloud] to be advertised"); - ros::service::waitForService("build_cloud"); - ROS_INFO("Found build_cloud! Starting the snapshotter"); - PeriodicSnapshotter snapshotter; - ros::spin(); + rclcpp::init(argc, argv); + auto service_name = std::string("periodic_snapshotter"); + char * cli_option = rcutils_cli_get_option(argv, argv + argc, "-s"); + if (nullptr != cli_option) { + service_name = std::string(cli_option); + } + auto node = std::make_shared(service_name); + rclcpp::spin(node); return 0; } diff --git a/include/laser_assembler/base_assembler.h b/include/laser_assembler/base_assembler.h deleted file mode 100644 index 2481dd7..0000000 --- a/include/laser_assembler/base_assembler.h +++ /dev/null @@ -1,373 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - -//! \author Vijay Pradeep - -#include "ros/ros.h" -#include "tf/transform_listener.h" -#include "tf/message_filter.h" -#include "sensor_msgs/PointCloud.h" -#include "sensor_msgs/point_cloud_conversion.h" -#include "message_filters/subscriber.h" - -#include - -// Service -#include "laser_assembler/AssembleScans.h" -#include "laser_assembler/AssembleScans2.h" - -#include "boost/thread.hpp" -#include "math.h" - -namespace laser_assembler -{ - -/** - * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request - */ -template -class BaseAssembler -{ -public: - BaseAssembler(const std::string& max_size_param_name); - ~BaseAssembler() ; - - /** - * \brief Tells the assembler to start listening to input data - * It is possible that a derived class needs to initialize and configure - * various things before actually beginning to process scans. Calling start - * create subcribes to the input stream, thus allowing the scanCallback and - * ConvertToCloud to be called. Start should be called only once. - */ - void start(const std::string& in_topic_name); - void start(); - - - /** \brief Returns the number of points in the current scan - * \param scan The scan for for which we want to know the number of points - * \return the number of points in scan - */ - virtual unsigned int GetPointsInScan(const T& scan) = 0 ; - - /** \brief Converts the current scan into a cloud in the specified fixed frame - * - * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. These exceptions are caught by - * BaseAssembler, and will be counted for diagnostic information - * \param fixed_frame_id The name of the frame in which we want cloud_out to be in - * \param scan_in The scan that we want to convert - * \param cloud_out The result of transforming scan_in into a cloud in frame fixed_frame_id - */ - virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ; - -protected: - tf::TransformListener* tf_ ; - tf::MessageFilter* tf_filter_; - - ros::NodeHandle private_ns_; - ros::NodeHandle n_; - -private: - // ROS Input/Ouptut Handling - ros::ServiceServer build_cloud_server_; - ros::ServiceServer assemble_scans_server_; - ros::ServiceServer build_cloud_server2_; - ros::ServiceServer assemble_scans_server2_; - message_filters::Subscriber scan_sub_; - message_filters::Connection tf_filter_connection_; - - //! \brief Callback function for every time we receive a new scan - //void scansCallback(const tf::MessageNotifier::MessagePtr& scan_ptr, const T& testA) - virtual void msgCallback(const boost::shared_ptr& scan_ptr) ; - - //! \brief Service Callback function called whenever we need to build a cloud - bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ; - bool assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) ; - bool buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; - bool assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) ; - - //! \brief Stores history of scans - std::deque scan_hist_ ; - boost::mutex scan_hist_mutex_ ; - - //! \brief The number points currently in the scan history - unsigned int total_pts_ ; - - //! \brief The max number of scans to store in the scan history - unsigned int max_scans_ ; - - //! \brief The frame to transform data into upon receipt - std::string fixed_frame_ ; - - //! \brief Specify how much to downsample the data. A value of 1 preserves all the data. 3 would keep 1/3 of the data. - unsigned int downsample_factor_ ; - -} ; - -template -BaseAssembler::BaseAssembler(const std::string& max_size_param_name) : private_ns_("~") -{ - // **** Initialize TransformListener **** - double tf_cache_time_secs ; - private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0) ; - if (tf_cache_time_secs < 0) - ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ; - - tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs)); - ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ; - - // ***** Set max_scans ***** - const int default_max_scans = 400 ; - int tmp_max_scans ; - private_ns_.param(max_size_param_name, tmp_max_scans, default_max_scans); - if (tmp_max_scans < 0) - { - ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ; - tmp_max_scans = default_max_scans ; - } - max_scans_ = tmp_max_scans ; - ROS_INFO("Max Scans in History: %u", max_scans_) ; - total_pts_ = 0 ; // We're always going to start with no points in our history - - // ***** Set fixed_frame ***** - private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")); - ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ; - if (fixed_frame_ == "ERROR_NO_NAME") - ROS_ERROR("Need to set parameter fixed_frame") ; - - // ***** Set downsample_factor ***** - int tmp_downsample_factor ; - private_ns_.param("downsample_factor", tmp_downsample_factor, 1); - if (tmp_downsample_factor < 1) - { - ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ; - tmp_downsample_factor = 1 ; - } - downsample_factor_ = tmp_downsample_factor ; - if (downsample_factor_ != 1) - ROS_WARN("Downsample set to [%u]. Note that this is an unreleased/unstable feature", downsample_factor_); - - // ***** Start Services ***** - build_cloud_server_ = n_.advertiseService("build_cloud", &BaseAssembler::buildCloud, this); - assemble_scans_server_ = n_.advertiseService("assemble_scans", &BaseAssembler::assembleScans, this); - build_cloud_server2_ = n_.advertiseService("build_cloud2", &BaseAssembler::buildCloud2, this); - assemble_scans_server2_ = n_.advertiseService("assemble_scans2", &BaseAssembler::assembleScans2, this); - - // ***** Start Listening to Data ***** - // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called) - tf_filter_ = NULL; - -} - -template -void BaseAssembler::start(const std::string& in_topic_name) -{ - ROS_DEBUG("Called start(string). Starting to listen on message_filter::Subscriber the input stream"); - if (tf_filter_) - ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ; - else - { - scan_sub_.subscribe(n_, in_topic_name, 10); - tf_filter_ = new tf::MessageFilter(scan_sub_, *tf_, fixed_frame_, 10); - tf_filter_->registerCallback( boost::bind(&BaseAssembler::msgCallback, this, _1) ); - } -} - -template -void BaseAssembler::start() -{ - ROS_DEBUG("Called start(). Starting tf::MessageFilter, but not initializing Subscriber"); - if (tf_filter_) - ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ; - else - { - scan_sub_.subscribe(n_, "bogus", 10); - tf_filter_ = new tf::MessageFilter(scan_sub_, *tf_, fixed_frame_, 10); - tf_filter_->registerCallback( boost::bind(&BaseAssembler::msgCallback, this, _1) ); - } -} - -template -BaseAssembler::~BaseAssembler() -{ - if (tf_filter_) - delete tf_filter_; - - delete tf_ ; -} - -template -void BaseAssembler::msgCallback(const boost::shared_ptr& scan_ptr) -{ - ROS_DEBUG("starting msgCallback"); - const T scan = *scan_ptr ; - - sensor_msgs::PointCloud cur_cloud ; - - // Convert the scan data into a universally known datatype: PointCloud - try - { - ConvertToCloud(fixed_frame_, scan, cur_cloud) ; // Convert scan into a point cloud - } - catch(tf::TransformException& ex) - { - ROS_WARN("Transform Exception %s", ex.what()) ; - return ; - } - - // Add the current scan (now of type PointCloud) into our history of scans - scan_hist_mutex_.lock() ; - if (scan_hist_.size() == max_scans_) // Is our deque full? - { - total_pts_ -= scan_hist_.front().points.size () ; // We're removing an elem, so this reduces our total point count - scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it - } - scan_hist_.push_back(cur_cloud) ; // Add the newest scan to the back of the deque - total_pts_ += cur_cloud.points.size () ; // Add the new scan to the running total of points - - //printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ; - - scan_hist_mutex_.unlock() ; - ROS_DEBUG("done with msgCallback"); -} - -template -bool BaseAssembler::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) -{ - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); - return assembleScans(req, resp); -} - - -template -bool BaseAssembler::assembleScans(AssembleScans::Request& req, AssembleScans::Response& resp) -{ - //printf("Starting Service Request\n") ; - - scan_hist_mutex_.lock() ; - // Determine where in our history we actually are - unsigned int i = 0 ; - - // Find the beginning of the request. Probably should be a search - while ( i < scan_hist_.size() && // Don't go past end of deque - scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time - { - i++ ; - } - unsigned int start_index = i ; - - unsigned int req_pts = 0 ; // Keep a total of the points in the current request - // Find the end of the request - while ( i < scan_hist_.size() && // Don't go past end of deque - scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request - { - req_pts += (scan_hist_[i].points.size ()+downsample_factor_-1)/downsample_factor_ ; - i += downsample_factor_ ; - } - unsigned int past_end_index = i ; - - if (start_index == past_end_index) - { - resp.cloud.header.frame_id = fixed_frame_ ; - resp.cloud.header.stamp = req.end ; - resp.cloud.points.resize (0) ; - resp.cloud.channels.resize (0) ; - } - else - { - // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen - // Allocate space for the cloud - resp.cloud.points.resize (req_pts); - const unsigned int num_channels = scan_hist_[start_index].channels.size (); - resp.cloud.channels.resize (num_channels) ; - for (i = 0; i%u. BufferSize: %lu. Points in cloud: %u", start_index, past_end_index, scan_hist_.size(), (int)resp.cloud.points.size ()) ; - return true ; -} - -template -bool BaseAssembler::buildCloud2(AssembleScans2::Request& req, AssembleScans2::Response& resp) -{ - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); - return assembleScans2(req, resp); -} - -template -bool BaseAssembler::assembleScans2(AssembleScans2::Request& req, AssembleScans2::Response& resp) -{ - AssembleScans::Request tmp_req; - AssembleScans::Response tmp_res; - tmp_req.begin = req.begin; - tmp_req.end = req.end; - bool ret = assembleScans(tmp_req, tmp_res); - - if ( ret ) - { - sensor_msgs::convertPointCloudToPointCloud2(tmp_res.cloud, resp.cloud); - } - return ret; -} -} diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp new file mode 100644 index 0000000..ac8149b --- /dev/null +++ b/include/laser_assembler/base_assembler.hpp @@ -0,0 +1,395 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. + +//! \author Vijay Pradeep + +#ifndef LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ +#define LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ + +#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/utilities.hpp" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2/buffer_core.h" +#include "tf2/time.h" +// #include "tf2_ros/message_filter.h" // TODO message_filter.h file in bouncy's +// tf2_ros package is not ported to ros2 yet. Found ported message_filter.h file +// on link +// https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/message_filter.h +// so copied that file to include directory of this package and fixed some +// errros. +#include "laser_assembler/message_filter.hpp" +#include "sensor_msgs/msg/point_cloud.hpp" +// #include "sensor_msgs/point_cloud_conversion.h" // TODO This file was not +// there in sensor_msgs package of ros2 so just copied it from ROS1 and ported to +// ROS2 here. +#include "message_filters/subscriber.h" +#include "laser_assembler/point_cloud_conversion.hpp" + +// Service +#include "laser_assembler/srv/assemble_scans.hpp" +#include "laser_assembler/srv/assemble_scans2.hpp" + +rclcpp::Logger g_logger = rclcpp::get_logger("laser_scan_assembler"); + +namespace laser_assembler +{ + +/** + * \brief Maintains a history of point clouds and generates an aggregate point + * cloud upon request + */ +template +class BaseAssembler +{ +public: + BaseAssembler( + const std::string & max_size_param_name, + const rclcpp::Node::SharedPtr & node_); + ~BaseAssembler(); + + /** + * \brief Tells the assembler to start listening to input data + * It is possible that a derived class needs to initialize and configure + * various things before actually beginning to process scans. Calling start + * create subcribes to the input stream, thus allowing the scanCallback and + * ConvertToCloud to be called. Start should be called only once. + */ + void start(const std::string & in_topic_name); + void start(); + + /** \brief Returns the number of points in the current scan + * \param scan The scan for for which we want to know the number of points + * \return the number of points in scan + */ + virtual unsigned int GetPointsInScan(const T & scan) = 0; + + /** \brief Converts the current scan into a cloud in the specified fixed frame + * + * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. + * These exceptions are caught by BaseAssembler, and will be counted for + * diagnostic information \param fixed_frame_id The name of the frame in which + * we want cloud_out to be in \param scan_in The scan that we want to convert + * \param cloud_out The result of transforming scan_in into a cloud in frame + * fixed_frame_id + */ + virtual void ConvertToCloud( + const std::string & fixed_frame_id, + const T & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) = 0; + +protected: + // message_filters's subscribe method requires raw node pointer. + rclcpp::Node::SharedPtr n_; + tf2::BufferCore tfBuffer; + tf2_ros::TransformListener * tf_; + tf2_ros::MessageFilter * tf_filter_; + +private: + // ROS Input/Ouptut Handling + rclcpp::Service::SharedPtr + assemble_scans_server_; + + message_filters::Subscriber scan_sub_; + + //! \brief Callback function for every time we receive a new scan + virtual void msgCallback(const std::shared_ptr & scan_ptr); + + //! \brief Service Callback function called whenever we need to build a cloud + bool assembleScans( + std::shared_ptr + request, + std::shared_ptr + response); + + //! \brief Stores history of scans + std::deque scan_hist_; + std::mutex scan_hist_mutex_; + + //! \brief The number points currently in the scan history + unsigned int total_pts_; + + //! \brief The max number of scans to store in the scan history + unsigned int max_scans_; + + //! \brief The frame to transform data into upon receipt + std::string fixed_frame_; + + //! \brief Specify how much to downsample the data. A value of 1 preserves all + //! the data. 3 would keep 1/3 of the data. + unsigned int downsample_factor_; +}; + +template +BaseAssembler::BaseAssembler( + const std::string & max_size_param_name, + const rclcpp::Node::SharedPtr & node_) +{ + // **** Initialize TransformListener **** + double tf_cache_time_secs; + n_ = node_; + + n_->get_parameter_or("tf_cache_time_secs", tf_cache_time_secs, 10.0); + + if (tf_cache_time_secs < 0) { + RCLCPP_ERROR(g_logger, "Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs); + } + + tf_ = new tf2_ros::TransformListener(tfBuffer); + RCLCPP_INFO(g_logger, "TF Cache Time: %f Seconds ", tf_cache_time_secs); + + // ***** Set max_scans ***** + const int default_max_scans = 400; + int tmp_max_scans; + + n_->get_parameter_or(max_size_param_name, tmp_max_scans, + default_max_scans); + + if (tmp_max_scans < 0) { + RCLCPP_ERROR(g_logger, "Parameter max_scans<0 (%i)", tmp_max_scans); + tmp_max_scans = default_max_scans; + } + max_scans_ = tmp_max_scans; + RCLCPP_INFO(g_logger, "Max Scans in History: %u ", max_scans_); + total_pts_ = 0; // We're always going to start with no points in our history + + // ***** Set fixed_frame ***** + n_->get_parameter_or("fixed_frame", fixed_frame_, + std::string("ERROR_NO_NAME")); + + RCLCPP_INFO(g_logger, "Fixed Frame: %s ", fixed_frame_.c_str()); + + if (fixed_frame_ == "ERROR_NO_NAME") { + RCLCPP_ERROR(g_logger, "Need to set parameter fixed_frame"); + } + + // ***** Set downsample_factor ***** + int tmp_downsample_factor; + n_->get_parameter_or("downsample_factor", tmp_downsample_factor, 1); + if (tmp_downsample_factor < 1) { + RCLCPP_ERROR(g_logger, "Parameter downsample_factor<1: %i", tmp_downsample_factor); + tmp_downsample_factor = 1; + } + downsample_factor_ = tmp_downsample_factor; + if (downsample_factor_ != 1) { + RCLCPP_WARN(g_logger, "Downsample set to [%u]. Note that this is an unreleased/unstable " + "feature", + downsample_factor_); + } + + // ***** Start Services ***** + auto assembleScansCallback = + [this]( + std::shared_ptr + request, + std::shared_ptr + response) -> bool { + this->assembleScans(request, response); + return true; + }; + + assemble_scans_server_ = + n_->create_service( + "assemble_scans", assembleScansCallback); + + // ***** Start Listening to Data ***** + // (Well, don't start listening just yet. Keep this as null until we actually + // start listening, when start() is called) + tf_filter_ = NULL; +} + +template +void BaseAssembler::start(const std::string & in_topic_name) +{ + RCLCPP_DEBUG(g_logger, "Called start(string). Starting to listen on " + "message_filter::Subscriber the input stream"); + if (tf_filter_) { + RCLCPP_ERROR(g_logger, "assembler::start() was called twice!. This is bad, and could " + "leak memory"); + } else { + // subscribe method requires raw node pointer. + scan_sub_.subscribe(n_.get(), in_topic_name); + tf_filter_ = + new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); + tf_filter_->registerCallback( + std::bind(&BaseAssembler::msgCallback, this, std::placeholders::_1)); + } +} + +template +void BaseAssembler::start() +{ + RCLCPP_DEBUG(g_logger, "Called start(). Starting tf::MessageFilter, but not initializing " + "Subscriber"); + if (tf_filter_) { + RCLCPP_ERROR(g_logger, "assembler::start() was called twice!. This is bad, and could " + "leak memory"); + } else { + scan_sub_.subscribe(n_.get(), "bogus"); + tf_filter_ = + new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); + tf_filter_->registerCallback( + std::bind(&BaseAssembler::msgCallback, this, std::placeholders::_1)); + } +} + +template +BaseAssembler::~BaseAssembler() +{ + if (tf_filter_) { + delete tf_filter_; + } + + delete tf_; +} + +template +void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) +{ + RCLCPP_DEBUG(g_logger, "starting msgCallback"); + const T scan = *scan_ptr; + + sensor_msgs::msg::PointCloud cur_cloud; + + // Convert the scan data into a universally known datatype: PointCloud + try { + ConvertToCloud(fixed_frame_, scan, + cur_cloud); // Convert scan into a point cloud + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(g_logger, "Transform Exception %s", ex.what()); + return; + } + + // Add the current scan (now of type PointCloud) into our history of scans + scan_hist_mutex_.lock(); + if (scan_hist_.size() == max_scans_) { // Is our deque full? + total_pts_ -= + scan_hist_.front().points.size(); // We're removing an elem, so this + // reduces our total point count + scan_hist_.pop_front(); // The front of the deque has the oldest elem, so we + // can get rid of it + } + scan_hist_.push_back( + cur_cloud); // Add the newest scan to the back of the deque + total_pts_ += cur_cloud.points + .size(); // Add the new scan to the running total of points + + // printf("Scans: %4lu Points: %10u\n", scan_hist_.size(), total_pts_); + + scan_hist_mutex_.unlock(); + RCLCPP_DEBUG(g_logger, "done with msgCallback"); +} + +template +bool BaseAssembler::assembleScans( + std::shared_ptr req, + std::shared_ptr + resp) +{ + scan_hist_mutex_.lock(); + // Determine where in our history we actually are + unsigned int i = 0; + + if (scan_hist_.size() <= 0) { + return true; + } + + // Find the beginning of the request. Probably should be a search + while (i < scan_hist_.size() && // Don't go past end of deque + scan_hist_[i].header.stamp.sec < + req->begin) // Keep stepping until we've exceeded the start time + { + i++; + } + unsigned int start_index = i; + + unsigned int req_pts = 0; // Keep a total of the points in the current request + // Find the end of the request + while (i < scan_hist_.size() && // Don't go past end of deque + ((scan_hist_[i].header.stamp.sec) < + req->end)) // Don't go past the end-time of the request + { + req_pts += (scan_hist_[i].points.size() + downsample_factor_ - 1) / + downsample_factor_; + i += downsample_factor_; + } + unsigned int past_end_index = i; + + if (start_index == past_end_index) { + resp->cloud.header.frame_id = fixed_frame_; + resp->cloud.header.stamp.sec = req->end; + resp->cloud.points.resize(0); + resp->cloud.channels.resize(0); + } else { + // Note: We are assuming that channel information is consistent across + // multiple scans. If not, then bad things (segfaulting) will happen + // Allocate space for the cloud + resp->cloud.points.resize(req_pts); + const unsigned int num_channels = scan_hist_[start_index].channels.size(); + resp->cloud.channels.resize(num_channels); + for (i = 0; i < num_channels; i++) { + resp->cloud.channels[i].name = scan_hist_[start_index].channels[i].name; + resp->cloud.channels[i].values.resize(req_pts); + } + // resp.cloud.header.stamp = req.end ; + resp->cloud.header.frame_id = fixed_frame_; + unsigned int cloud_count = 0; + for (i = start_index; i < past_end_index; i += downsample_factor_) { + // Sanity check: Each channel should be the same length as the points + // vector + for (unsigned int chan_ind = 0; chan_ind < scan_hist_[i].channels.size(); + chan_ind++) + { + if (scan_hist_[i].points.size() != + scan_hist_[i].channels[chan_ind].values.size()) + { + RCLCPP_FATAL(g_logger, "Trying to add a malformed point cloud. Cloud has %u " + "points, but channel %u has %u elems", + (int)scan_hist_[i].points.size(), chan_ind, + (int)scan_hist_[i].channels[chan_ind].values.size()); + } + } + + for (unsigned int j = 0; j < scan_hist_[i].points.size(); + j += downsample_factor_) + { + resp->cloud.points[cloud_count].x = scan_hist_[i].points[j].x; + resp->cloud.points[cloud_count].y = scan_hist_[i].points[j].y; + resp->cloud.points[cloud_count].z = scan_hist_[i].points[j].z; + + for (unsigned int k = 0; k < num_channels; k++) { + resp->cloud.channels[k].values[cloud_count] = + scan_hist_[i].channels[k].values[j]; + } + + cloud_count++; + } + resp->cloud.header.stamp = scan_hist_[i].header.stamp; + } + } + scan_hist_mutex_.unlock(); + + RCLCPP_DEBUG(g_logger, "\nPoint Cloud Results: Aggregated from index %u->%u. BufferSize: " + "%lu. Points in cloud: %u", + start_index, past_end_index, scan_hist_.size(), + (int)resp->cloud.points.size()); + return true; +} +} // namespace laser_assembler +#endif // LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ diff --git a/include/laser_assembler/base_assembler_srv.h b/include/laser_assembler/base_assembler_srv.h deleted file mode 100644 index 36dbb64..0000000 --- a/include/laser_assembler/base_assembler_srv.h +++ /dev/null @@ -1,330 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - -//! \author Vijay Pradeep - -#include "ros/ros.h" -#include "tf/transform_listener.h" -#include "tf/message_filter.h" -#include "sensor_msgs/PointCloud.h" -#include "message_filters/subscriber.h" - -#include - -// Service -#include "laser_assembler/AssembleScans.h" - -#include "boost/thread.hpp" -#include "math.h" - -namespace laser_assembler -{ - -/** - * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request - * \todo Clean up the doxygen part of this header - * - * @section parameters ROS Parameters - * - * Reads the following parameters from the parameter server - * - \b "~tf_cache_time_secs" (double) - The cache time (in seconds) to holds past transforms - * - \b "~tf_tolerance_secs (double) - The time (in seconds) to wait after the transform for scan_in is available. - * - \b "~max_scans" (unsigned int) - The number of scans to store in the assembler's history, until they're thrown away - * - \b "~fixed_frame" (string) - The frame to which received data should immeadiately be transformed to - * - \b "~downsampling_factor" (int) - Specifies how often to sample from a scan. 1 preserves all the data. 3 keeps only 1/3 of the points. - * - * @section services ROS Service Calls - * - \b "~build_cloud" (AssembleScans.srv) - Accumulates scans between begin time and - * end time and returns the aggregated data as a point cloud - */ -template -class BaseAssemblerSrv -{ -public: - BaseAssemblerSrv() ; - ~BaseAssemblerSrv() ; - - /** - * \brief Tells the assembler to start listening to input data - * It is possible that a derived class needs to initialize and configure - * various things before actually beginning to process scans. Calling start - * create subcribes to the input stream, thus allowing the scanCallback and - * ConvertToCloud to be called. Start should be called only once. - */ - void start() ; - - - /** \brief Returns the number of points in the current scan - * \param scan The scan for for which we want to know the number of points - * \return the number of points in scan - */ - virtual unsigned int GetPointsInScan(const T& scan) = 0 ; - - /** \brief Converts the current scan into a cloud in the specified fixed frame - * - * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. These exceptions are caught by - * BaseAssemblerSrv, and will be counted for diagnostic information - * \param fixed_frame_id The name of the frame in which we want cloud_out to be in - * \param scan_in The scan that we want to convert - * \param cloud_out The result of transforming scan_in into a cloud in frame fixed_frame_id - */ - virtual void ConvertToCloud(const std::string& fixed_frame_id, const T& scan_in, sensor_msgs::PointCloud& cloud_out) = 0 ; - -protected: - tf::TransformListener* tf_ ; - - ros::NodeHandle private_ns_; - ros::NodeHandle n_; - -private: - // ROS Input/Ouptut Handling - ros::ServiceServer cloud_srv_server_; - message_filters::Subscriber scan_sub_; - tf::MessageFilter* tf_filter_; - message_filters::Connection tf_filter_connection_; - - //! \brief Callback function for every time we receive a new scan - //void scansCallback(const tf::MessageNotifier::MessagePtr& scan_ptr, const T& testA) - void scansCallback(const boost::shared_ptr& scan_ptr) ; - - //! \brief Service Callback function called whenever we need to build a cloud - bool buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) ; - - - //! \brief Stores history of scans - std::deque scan_hist_ ; - boost::mutex scan_hist_mutex_ ; - - //! \brief The number points currently in the scan history - unsigned int total_pts_ ; - - //! \brief The max number of scans to store in the scan history - unsigned int max_scans_ ; - - //! \brief The frame to transform data into upon receipt - std::string fixed_frame_ ; - - //! \brief How long we should wait before processing the input data. Very useful for laser scans. - double tf_tolerance_secs_ ; - - //! \brief Specify how much to downsample the data. A value of 1 preserves all the data. 3 would keep 1/3 of the data. - unsigned int downsample_factor_ ; - -} ; - -template -BaseAssemblerSrv::BaseAssemblerSrv() : private_ns_("~") -{ - // **** Initialize TransformListener **** - double tf_cache_time_secs ; - private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0) ; - if (tf_cache_time_secs < 0) - ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs) ; - - tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs)); - ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs) ; - - // ***** Set max_scans ***** - const int default_max_scans = 400 ; - int tmp_max_scans ; - private_ns_.param("max_scans", tmp_max_scans, default_max_scans); - if (tmp_max_scans < 0) - { - ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans) ; - tmp_max_scans = default_max_scans ; - } - max_scans_ = tmp_max_scans ; - ROS_INFO("Max Scans in History: %u", max_scans_) ; - total_pts_ = 0 ; // We're always going to start with no points in our history - - // ***** Set fixed_frame ***** - private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")); - ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()) ; - if (fixed_frame_ == "ERROR_NO_NAME") - ROS_ERROR("Need to set parameter fixed_frame") ; - - // ***** Set downsample_factor ***** - int tmp_downsample_factor ; - private_ns_.param("downsample_factor", tmp_downsample_factor, 1); - if (tmp_downsample_factor < 1) - { - ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor) ; - tmp_downsample_factor = 1 ; - } - downsample_factor_ = tmp_downsample_factor ; - ROS_INFO("Downsample Factor: %u", downsample_factor_) ; - - // ***** Start Services ***** - cloud_srv_server_ = private_ns_.advertiseService("build_cloud", &BaseAssemblerSrv::buildCloud, this); - - // **** Get the TF Notifier Tolerance **** - private_ns_.param("tf_tolerance_secs", tf_tolerance_secs_, 0.0); - if (tf_tolerance_secs_ < 0) - ROS_ERROR("Parameter tf_tolerance_secs<0 (%f)", tf_tolerance_secs_) ; - ROS_INFO("tf Tolerance: %f seconds", tf_tolerance_secs_) ; - - // ***** Start Listening to Data ***** - // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called) - tf_filter_ = NULL; - -} - -template -void BaseAssemblerSrv::start() -{ - ROS_INFO("Starting to listen on the input stream") ; - if (tf_filter_) - ROS_ERROR("assembler::start() was called twice!. This is bad, and could leak memory") ; - else - { - scan_sub_.subscribe(n_, "scan_in", 10); - tf_filter_ = new tf::MessageFilter(scan_sub_, *tf_, fixed_frame_, 10); - tf_filter_->setTolerance(ros::Duration(tf_tolerance_secs_)); - tf_filter_->registerCallback( boost::bind(&BaseAssemblerSrv::scansCallback, this, _1) ); - } -} - -template -BaseAssemblerSrv::~BaseAssemblerSrv() -{ - if (tf_filter_) - delete tf_filter_; - - delete tf_ ; -} - -template -void BaseAssemblerSrv::scansCallback(const boost::shared_ptr& scan_ptr) -{ - const T scan = *scan_ptr ; - - sensor_msgs::PointCloud cur_cloud ; - - // Convert the scan data into a universally known datatype: PointCloud - try - { - ConvertToCloud(fixed_frame_, scan, cur_cloud) ; // Convert scan into a point cloud - } - catch(tf::TransformException& ex) - { - ROS_WARN("Transform Exception %s", ex.what()) ; - return ; - } - - // Add the current scan (now of type PointCloud) into our history of scans - scan_hist_mutex_.lock() ; - if (scan_hist_.size() == max_scans_) // Is our deque full? - { - total_pts_ -= scan_hist_.front().points.size() ; // We're removing an elem, so this reduces our total point count - scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it - } - scan_hist_.push_back(cur_cloud) ; // Add the newest scan to the back of the deque - total_pts_ += cur_cloud.points.size() ; // Add the new scan to the running total of points - - //printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ; - - scan_hist_mutex_.unlock() ; -} - -template -bool BaseAssemblerSrv::buildCloud(AssembleScans::Request& req, AssembleScans::Response& resp) -{ - //printf("Starting Service Request\n") ; - - scan_hist_mutex_.lock() ; - // Determine where in our history we actually are - unsigned int i = 0 ; - - // Find the beginning of the request. Probably should be a search - while ( i < scan_hist_.size() && // Don't go past end of deque - scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time - { - i++ ; - } - unsigned int start_index = i ; - - unsigned int req_pts = 0 ; // Keep a total of the points in the current request - // Find the end of the request - while ( i < scan_hist_.size() && // Don't go past end of deque - scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request - { - req_pts += (scan_hist_[i].points.size()+downsample_factor_-1)/downsample_factor_ ; - i += downsample_factor_ ; - } - unsigned int past_end_index = i ; - - if (start_index == past_end_index) - { - resp.cloud.header.frame_id = fixed_frame_ ; - resp.cloud.header.stamp = req.end ; - resp.cloud.points.resize(0) ; - resp.cloud.channels.resize(0) ; - } - else - { - // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen - // Allocate space for the cloud - resp.cloud.points.resize( req_pts ) ; - const unsigned int num_channels = scan_hist_[start_index].channels.size() ; - resp.cloud.channels.resize(num_channels) ; - for (i = 0; i%u. BufferSize: %lu. Points in cloud: %lu", start_index, past_end_index, scan_hist_.size(), resp.cloud.points.size()) ; - return true ; -} - -} diff --git a/include/laser_assembler/base_assembler_srv.hpp b/include/laser_assembler/base_assembler_srv.hpp new file mode 100644 index 0000000..5403003 --- /dev/null +++ b/include/laser_assembler/base_assembler_srv.hpp @@ -0,0 +1,337 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. + + +//! \author Vijay Pradeep + +#ifndef LASER_ASSEMBLER__BASE_ASSEMBLER_SRV_HPP_ +#define LASER_ASSEMBLER__BASE_ASSEMBLER_SRV_HPP_ + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_listener.h" +#include "laser_assembler/message_filter.hpp" +#include "message_filters/subscriber.h" +#include "sensor_msgs/msg/point_cloud.hpp" + +// Service +#include "laser_assembler/srv/assemble_scans.hpp" + +namespace laser_assembler +{ + +/** + * \brief Maintains a history of point clouds and generates an aggregate point + * cloud upon request \todo Clean up the doxygen part of this header + * + * @section parameters ROS Parameters + * + * Reads the following parameters from the parameter server + * - \b "~tf_cache_time_secs" (double) - The cache time (in seconds) to holds + * past transforms + * - \b "~tf_tolerance_secs (double) - The time (in seconds) to wait after the + * transform for scan_in is available. + * - \b "~max_scans" (unsigned int) - The number of scans to store in the + * assembler's history, until they're thrown away + * - \b "~fixed_frame" (string) - The frame to which received data should + * immeadiately be transformed to + * - \b "~downsampling_factor" (int) - Specifies how often to sample from a + * scan. 1 preserves all the data. 3 keeps only 1/3 of the points. + * + * @section services ROS Service Calls + * - \b "~build_cloud" (AssembleScans.srv) - Accumulates scans between begin + * time and end time and returns the aggregated data as a point cloud + */ +template +class BaseAssemblerSrv +{ +public: + BaseAssemblerSrv(); + ~BaseAssemblerSrv(); + + /** + * \brief Tells the assembler to start listening to input data + * It is possible that a derived class needs to initialize and configure + * various things before actually beginning to process scans. Calling start + * create subcribes to the input stream, thus allowing the scanCallback and + * ConvertToCloud to be called. Start should be called only once. + */ + void start(); + + /** \brief Returns the number of points in the current scan + * \param scan The scan for for which we want to know the number of points + * \return the number of points in scan + */ + virtual unsigned int GetPointsInScan(const T & scan) = 0; + + /** \brief Converts the current scan into a cloud in the specified fixed frame + * + * Note: Once implemented, ConvertToCloud should NOT catch TF exceptions. + * These exceptions are caught by BaseAssemblerSrv, and will be counted for + * diagnostic information \param fixed_frame_id The name of the frame in which + * we want cloud_out to be in \param scan_in The scan that we want to convert + * \param cloud_out The result of transforming scan_in into a cloud in frame + * fixed_frame_id + */ + virtual void ConvertToCloud( + const std::string & fixed_frame_id, + const T & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) = 0; + +protected: + tf::TransformListener * tf_; + + ros::NodeHandle private_ns_; + ros::NodeHandle n_; + +private: + // ROS Input/Ouptut Handling + ros::ServiceServer cloud_srv_server_; + message_filters::Subscriber scan_sub_; + tf::MessageFilter * tf_filter_; + message_filters::Connection tf_filter_connection_; + + //! \brief Callback function for every time we receive a new scan + void scansCallback(const boost::shared_ptr & scan_ptr); + + //! \brief Service Callback function called whenever we need to build a cloud + bool buildCloud(assemble_scans::Request & req, assemble_scans::Response & resp); + + //! \brief Stores history of scans + std::deque scan_hist_; + boost::mutex scan_hist_mutex_; + + //! \brief The number points currently in the scan history + unsigned int total_pts_; + + //! \brief The max number of scans to store in the scan history + unsigned int max_scans_; + + //! \brief The frame to transform data into upon receipt + std::string fixed_frame_; + + //! \brief How long we should wait before processing the input data. Very + //! useful for laser scans. + double tf_tolerance_secs_; + + //! \brief Specify how much to downsample the data. A value of 1 preserves all + //! the data. 3 would keep 1/3 of the data. + unsigned int downsample_factor_; +}; + +template +BaseAssemblerSrv::BaseAssemblerSrv() +: private_ns_("~") +{ + // **** Initialize TransformListener **** + double tf_cache_time_secs; + private_ns_.param("tf_cache_time_secs", tf_cache_time_secs, 10.0); + if (tf_cache_time_secs < 0) { + RCLCPP_ERROR(n_->get_logger(), "Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs); + } + + tf_ = new tf::TransformListener(n_, ros::Duration(tf_cache_time_secs)); + RCLCPP_INFO(n_->get_logger(), "TF Cache Time: %f Seconds", tf_cache_time_secs); + + // ***** Set max_scans ***** + const int default_max_scans = 400; + int tmp_max_scans; + private_ns_.param("max_scans", tmp_max_scans, default_max_scans); + if (tmp_max_scans < 0) { + RCLCPP_ERROR(n_->get_logger(), "Parameter max_scans<0 (%i)", tmp_max_scans); + tmp_max_scans = default_max_scans; + } + max_scans_ = tmp_max_scans; + RCLCPP_INFO(n_->get_logger(), "Max Scans in History: %u", max_scans_); + total_pts_ = 0; // We're always going to start with no points in our history + + // ***** Set fixed_frame ***** + private_ns_.param("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")); + RCLCPP_INFO(n_->get_logger(), "Fixed Frame: %s", fixed_frame_.c_str()); + if (fixed_frame_ == "ERROR_NO_NAME") { + RCLCPP_ERROR(n_->get_logger(), "Need to set parameter fixed_frame"); + } + + // ***** Set downsample_factor ***** + int tmp_downsample_factor; + private_ns_.param("downsample_factor", tmp_downsample_factor, 1); + if (tmp_downsample_factor < 1) { + RCLCPP_ERROR(n_->get_logger(), "Parameter downsample_factor<1: %i", tmp_downsample_factor); + tmp_downsample_factor = 1; + } + downsample_factor_ = tmp_downsample_factor; + RCLCPP_INFO(n_->get_logger(), "Downsample Factor: %u", downsample_factor_); + + // ***** Start Services ***** + cloud_srv_server_ = private_ns_.advertiseService( + "build_cloud", &BaseAssemblerSrv::buildCloud, this); + + // **** Get the TF Notifier Tolerance **** + private_ns_.param("tf_tolerance_secs", tf_tolerance_secs_, 0.0); + if (tf_tolerance_secs_ < 0) { + RCLCPP_ERROR(n_->get_logger(), "Parameter tf_tolerance_secs<0 (%f)", tf_tolerance_secs_); + } + RCLCPP_INFO(n_->get_logger(), "tf Tolerance: %f seconds", tf_tolerance_secs_); + + // ***** Start Listening to Data ***** + // (Well, don't start listening just yet. Keep this as null until we actually + // start listening, when start() is called) + tf_filter_ = NULL; +} + +template +void BaseAssemblerSrv::start() +{ + RCLCPP_INFO(n_->get_logger(), "Starting to listen on the input stream"); + if (tf_filter_) { + RCLCPP_ERROR(n_->get_logger(), "assembler::start() was called twice!. This is bad, and could " + "leak memory"); + } else { + scan_sub_.subscribe(n_, "scan_in", 10); + tf_filter_ = new tf::MessageFilter(scan_sub_, *tf_, fixed_frame_, 10); + tf_filter_->setTolerance(ros::Duration(tf_tolerance_secs_)); + tf_filter_->registerCallback( + boost::bind(&BaseAssemblerSrv::scansCallback, this, _1)); + } +} + +template +BaseAssemblerSrv::~BaseAssemblerSrv() +{ + if (tf_filter_) { + delete tf_filter_; + } + + delete tf_; +} + +template +void BaseAssemblerSrv::scansCallback( + const boost::shared_ptr & scan_ptr) +{ + const T scan = *scan_ptr; + + sensor_msgs::msg::PointCloud cur_cloud; + + // Convert the scan data into a universally known datatype: PointCloud + try { + ConvertToCloud(fixed_frame_, scan, + cur_cloud); // Convert scan into a point cloud + } catch (tf::TransformException & ex) { + RCLCPP_WARN(n_->get_logger(), "Transform Exception %s", ex.what()); + return; + } + + // Add the current scan (now of type PointCloud) into our history of scans + scan_hist_mutex_.lock(); + if (scan_hist_.size() == max_scans_) { // Is our deque full? + total_pts_ -= + scan_hist_.front().points.size(); // We're removing an elem, so this + // reduces our total point count + scan_hist_.pop_front(); // The front of the deque has the oldest elem, so we + // can get rid of it + } + scan_hist_.push_back( + cur_cloud); // Add the newest scan to the back of the deque + total_pts_ += cur_cloud.points + .size(); // Add the new scan to the running total of points + + // printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ; + + scan_hist_mutex_.unlock(); +} + +template +bool BaseAssemblerSrv::buildCloud( + assemble_scans::Request & req, + assemble_scans::Response & resp) +{ + // printf("Starting Service Request\n") ; + + scan_hist_mutex_.lock(); + // Determine where in our history we actually are + unsigned int i = 0; + + // Find the beginning of the request. Probably should be a search + while (i < scan_hist_.size() && // Don't go past end of deque + scan_hist_[i].header.stamp < + req.begin) // Keep stepping until we've exceeded the start time + { + i++; + } + unsigned int start_index = i; + + unsigned int req_pts = 0; // Keep a total of the points in the current request + // Find the end of the request + while (i < scan_hist_.size() && // Don't go past end of deque + scan_hist_[i].header.stamp < + req.end) // Don't go past the end-time of the request + { + req_pts += (scan_hist_[i].points.size() + downsample_factor_ - 1) / + downsample_factor_; + i += downsample_factor_; + } + unsigned int past_end_index = i; + + if (start_index == past_end_index) { + resp.cloud.header.frame_id = fixed_frame_; + resp.cloud.header.stamp = req.end; + resp.cloud.points.resize(0); + resp.cloud.channels.resize(0); + } else { + // Note: We are assuming that channel information is consistent across + // multiple scans. If not, then bad things (segfaulting) will happen + // Allocate space for the cloud + resp.cloud.points.resize(req_pts); + const unsigned int num_channels = scan_hist_[start_index].channels.size(); + resp.cloud.channels.resize(num_channels); + for (i = 0; i < num_channels; i++) { + resp.cloud.channels[i].name = scan_hist_[start_index].channels[i].name; + resp.cloud.channels[i].values.resize(req_pts); + } + // resp.cloud.header.stamp = req.end ; + resp.cloud.header.frame_id = fixed_frame_; + unsigned int cloud_count = 0; + for (i = start_index; i < past_end_index; i += downsample_factor_) { + for (unsigned int j = 0; j < scan_hist_[i].points.size(); + j += downsample_factor_) + { + resp.cloud.points[cloud_count].x = scan_hist_[i].points[j].x; + resp.cloud.points[cloud_count].y = scan_hist_[i].points[j].y; + resp.cloud.points[cloud_count].z = scan_hist_[i].points[j].z; + for (unsigned int k = 0; k < num_channels; k++) { + resp.cloud.channels[k].values[cloud_count] = + scan_hist_[i].channels[k].values[j]; + } + + cloud_count++; + } + resp.cloud.header.stamp = scan_hist_[i].header.stamp; + } + } + scan_hist_mutex_.unlock(); + + RCLCPP_DEBUG(n_->get_logger(), "Point Cloud Results: Aggregated from index %u->%u. BufferSize: " + "%lu. Points in cloud: %lu", + start_index, past_end_index, scan_hist_.size(), + resp.cloud.points.size()); + return true; +} + +} // namespace laser_assembler + +#endif // LASER_ASSEMBLER__BASE_ASSEMBLER_SRV_HPP_ diff --git a/include/laser_assembler/message_filter.hpp b/include/laser_assembler/message_filter.hpp new file mode 100644 index 0000000..9dbaaf9 --- /dev/null +++ b/include/laser_assembler/message_filter.hpp @@ -0,0 +1,694 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. + +/** \author Josh Faust */ + +#ifndef TF2_ROS_MESSAGE_FILTER_H // NOLINT +#define TF2_ROS_MESSAGE_FILTER_H // NOLINT + +#include +#include +#include +#include +#include +#include + +#include "message_filters/connection.h" +#include "message_filters/message_traits.h" +#include "message_filters/simple_filter.h" +#include "rclcpp/rclcpp.hpp" +#include "tf2/buffer_core.h" + +#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ + RCUTILS_LOG_DEBUG_NAMED("tf2_ros_message_filter", \ + std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \ + getTargetFramesString().c_str(), __VA_ARGS__) + +#define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \ + RCUTILS_LOG_WARN_NAMED("tf2_ros_message_filter", \ + std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \ + getTargetFramesString().c_str(), __VA_ARGS__) + +namespace tf2_ros +{ + +namespace filter_failure_reasons +{ +enum FilterFailureReason +{ + /// The message buffer overflowed, and this message + /// was pushed off the back of the queue, + /// but the reason it was unable to be transformed is unknown. + Unknown, + /// The timestamp on the message is more than the cache length + /// earlier than the newest data in the transform cache + OutTheBack, + /// The frame_id on the message is empty + EmptyFrameID, +}; +} // namespace filter_failure_reasons + +typedef filter_failure_reasons::FilterFailureReason FilterFailureReason; + +class MessageFilterBase +{ +public: + typedef std::vector V_string; + virtual ~MessageFilterBase() {} + virtual void clear() = 0; + virtual void setTargetFrame(const std::string & target_frame) = 0; + virtual void setTargetFrames(const V_string & target_frames) = 0; + virtual void setTolerance(const rclcpp::Duration & tolerance) = 0; +}; + +/** + * \brief Follows the patterns set by the message_filters package + * to implement a filter which only passes messages + * through once there is transform data available + * + * The callbacks used in this class are of the same form + * as those used by rclcpp's message callbacks. + * + * MessageFilter is templated on a message type. + * + * \section example_usage Example Usage + * + * If you want to hook a MessageFilter into a ROS topic: + \verbatim + message_filters::Subscriber sub(node_, "topic", 10); + tf::MessageFilter tf_filter(sub, tf_listener_, "/map", 10); + tf_filter.registerCallback(&MyClass::myCallback, this); + \endverbatim + */ +template +class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter +{ +public: + using MConstPtr = std::shared_ptr; + typedef message_filters::MessageEvent MEvent; + // typedef std::function FailureCallback; + + // If you hit this assert your message does not have a header, + // or does not have the HasHeader trait defined for it + // Actually, we need to check that the message has a header, or that it + // has the FrameId and Stamp traits. However I don't know how to do that + // so simply commenting out for now. + // ROS_STATIC_ASSERT(ros::message_traits::HasHeader::value); + + /** + * \brief Constructor + * + * \param bc The tf2::BufferCore this filter should use + * \param target_frame The frame this filter should attempt to transform to. + * To use multiple frames, pass an empty string here + * and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing + * away old ones. 0 means infinite (dangerous). + * \param node The ros2 node whose callback queue we should add callbacks to + */ + MessageFilter( + tf2::BufferCore & bc, const std::string & target_frame, uint32_t queue_size, + const rclcpp::Node::SharedPtr & node) + : bc_(bc), + queue_size_(queue_size), + node_(node) + { + init(); + + setTargetFrame(target_frame); + } + + /** + * \brief Constructor + * + * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. + * \param bc The tf2::BufferCore this filter should use + * \param target_frame The frame this filter should attempt to transform to. + * To use multiple frames, pass an empty string here + * and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. + * 0 means infinite (dangerous). + * \param node The ros2 node whose callback queue we should add callbacks to + */ + template + MessageFilter( + F & f, tf2::BufferCore & bc, const std::string & target_frame, uint32_t queue_size, + const rclcpp::Node::SharedPtr & node) + : bc_(bc), + queue_size_(queue_size), + node_(node) + { + init(); + + setTargetFrame(target_frame); + + connectInput(f); + } + + /** + * \brief Connect this filter's input to another filter's output. + * If this filter is already connected, disconnects first. + */ + template + void connectInput(F & f) + { + message_connection_.disconnect(); + message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); + } + + /** + * \brief Destructor + */ + ~MessageFilter() + { + message_connection_.disconnect(); + clear(); + + TF2_ROS_MESSAGEFILTER_DEBUG( + "Successful Transforms: %llu, Discarded due to age: %llu, Transform " + "messages received: %llu, Messages received: %llu, Total dropped: %llu", + static_cast(successful_transform_count_), + static_cast(failed_out_the_back_count_), + static_cast(transform_message_count_), + static_cast(incoming_message_count_), + static_cast(dropped_message_count_)); + } + + /** + * \brief Set the frame you need to be able to transform + * to before getting a message callback + */ + void setTargetFrame(const std::string & target_frame) + { + V_string frames; + frames.push_back(target_frame); + setTargetFrames(frames); + } + + /** + * \brief Set the frames you need to be able to transform + * to before getting a message callback + */ + void setTargetFrames(const V_string & target_frames) + { + std::unique_lock frames_lock(target_frames_mutex_); + + target_frames_.resize(target_frames.size()); + std::transform(target_frames.begin(), target_frames.end(), + target_frames_.begin(), this->stripSlash); + expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); + + std::stringstream ss; + for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) { + ss << *it << " "; + } + target_frames_string_ = ss.str(); + } + + /** + * \brief Get the target frames as a string for debugging + */ + std::string getTargetFramesString() + { + std::unique_lock frames_lock(target_frames_mutex_); + return target_frames_string_; + } + + /** + * \brief Set the required tolerance for the notifier to return true + */ + void setTolerance(const rclcpp::Duration & tolerance) + { + std::unique_lock frames_lock(target_frames_mutex_); + time_tolerance_ = tolerance; + expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); + } + + /** + * \brief Clear any messages currently in the queue + */ + void clear() + { + std::unique_lock unique_lock(messages_mutex_); + + TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared"); + + bc_.removeTransformableCallback(callback_handle_); + callback_handle_ = bc_.addTransformableCallback(std::bind(&MessageFilter::transformable, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3, + std::placeholders::_4, + std::placeholders::_5)); + + messages_.clear(); + message_count_ = 0; + + warned_about_empty_frame_id_ = false; + } + + void add(const MEvent & evt) + { + if (target_frames_.empty()) { + return; + } + + namespace mt = message_filters::message_traits; + const MConstPtr & message = evt.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + rclcpp::Time stamp = mt::TimeStamp::value(*message); + + if (frame_id.empty()) { + messageDropped(evt, filter_failure_reasons::EmptyFrameID); + return; + } + + // iterate through the target frames + // and add requests for each of them + MessageInfo info; + info.handles.reserve(expected_success_count_); + { + V_string target_frames_copy; + // Copy target_frames_ to avoid deadlock from #79 + { + std::unique_lock frames_lock(target_frames_mutex_); + target_frames_copy = target_frames_; + } + + V_string::iterator it = target_frames_copy.begin(); + V_string::iterator end = target_frames_copy.end(); + for (; it != end; ++it) { + const std::string & target_frame = *it; + // class rclcpp::Time’ has no member named ‘seconds'. + // So used nanoseconds() and converted it to seconds, dividing it by 1e+9. + tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, + target_frame, frame_id, tf2::timeFromSec( + stamp.nanoseconds() / 1e+9)); + if (handle == 0xffffffffffffffffULL) { + // never transformable + messageDropped(evt, filter_failure_reasons::OutTheBack); + return; + } else if (handle == 0) { + ++info.success_count; + } else { + info.handles.push_back(handle); + } + + if (time_tolerance_.nanoseconds()) { + handle = bc_.addTransformableRequest(callback_handle_, + target_frame, frame_id, tf2::timeFromSec( + (stamp + time_tolerance_).nanoseconds() / 1e+9)); + if (handle == 0xffffffffffffffffULL) { + // never transformable + messageDropped(evt, filter_failure_reasons::OutTheBack); + return; + } else if (handle == 0) { + ++info.success_count; + } else { + info.handles.push_back(handle); + } + } + } + } + + // We can transform already + if (info.success_count == expected_success_count_) { + messageReady(evt); + } else { + // If this message is about to push us past + // our queue size, erase the oldest message + if (queue_size_ != 0 && message_count_ + 1 > queue_size_) { + // While we're using the reference keep a lock on the messages. + std::unique_lock unique_lock(messages_mutex_); + + ++dropped_message_count_; + const MessageInfo & front = messages_.front(); + TF2_ROS_MESSAGEFILTER_DEBUG( + "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", + message_count_, + (mt::FrameId::value(*front.event.getMessage())).c_str(), + (mt::TimeStamp::value(*front.event.getMessage()).nanoseconds() / 1e+9)); + + V_TransformableRequestHandle::const_iterator it = front.handles.begin(); + V_TransformableRequestHandle::const_iterator end = front.handles.end(); + for (; it != end; ++it) { + bc_.cancelTransformableRequest(*it); + } + + messageDropped(front.event, filter_failure_reasons::Unknown); + + messages_.pop_front(); + --message_count_; + } + + // Add the message to our list + info.event = evt; + messages_.push_back(info); + ++message_count_; + } + + TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", + frame_id.c_str(), (stamp.nanoseconds() / 1e+9), message_count_); + ++incoming_message_count_; + } + + /** + * \brief Manually add a message into this filter. + * \note If the message (or any other messages in the queue) + * are immediately transformable this will immediately + * call through to the output callback, possibly + * multiple times + */ + void add(const MConstPtr & message) + { + using builtin_interfaces::msg::Time; + std::shared_ptr> header(new std::map); + + (*header)["callerid"] = "unknown"; + // Time t = rclcpp::Clock::now(); + rclcpp::Clock system_clock(RCL_SYSTEM_TIME); + rclcpp::Time t = system_clock.now(); + add(MEvent(message, header, t)); + } + + /** + * \brief Register a callback to be called + * when a message is about to be dropped + * \param callback The callback to call + */ +#if 0 + message_filters::Connection + registerFailureCallback(const FailureCallback & callback) + { + message_connection_failure.disconnect(); + message_connection_failure = this->registerCallback(callback, this); + } +#endif + + virtual void setQueueSize(uint32_t new_queue_size) + { + queue_size_ = new_queue_size; + } + + virtual uint32_t getQueueSize() + { + return queue_size_; + } + +private: + void init() + { + message_count_ = 0; + successful_transform_count_ = 0; + failed_out_the_back_count_ = 0; + transform_message_count_ = 0; + incoming_message_count_ = 0; + dropped_message_count_ = 0; + time_tolerance_ = rclcpp::Duration(0, 0); + warned_about_empty_frame_id_ = false; + expected_success_count_ = 1; + + callback_handle_ = bc_.addTransformableCallback(std::bind(&MessageFilter::transformable, + this, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3, + std::placeholders::_4, + std::placeholders::_5)); + } + + void transformable( + tf2::TransformableRequestHandle request_handle, const std::string & target_frame, + const std::string & source_frame, + tf2::TimePoint time, tf2::TransformableResult result) + { + namespace mt = message_filters::message_traits; + + // find the message this request is associated with + typename L_MessageInfo::iterator msg_it = messages_.begin(); + typename L_MessageInfo::iterator msg_end = messages_.end(); + for (; msg_it != msg_end; ++msg_it) { + MessageInfo & info = *msg_it; + V_TransformableRequestHandle::const_iterator handle_it = std::find( + info.handles.begin(), info.handles.end(), request_handle); + if (handle_it != info.handles.end()) { + // found msg_it + ++info.success_count; + break; + } + } + + if (msg_it == msg_end) { + return; + } + + const MessageInfo & info = *msg_it; + if (info.success_count < expected_success_count_) { + return; + } + + bool can_transform = true; + const MConstPtr & message = info.event.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + rclcpp::Time stamp = mt::TimeStamp::value(*message); + + if (result == tf2::TransformAvailable) { + std::unique_lock frames_lock(target_frames_mutex_); + // make sure we can still perform all the necessary transforms + typename V_string::iterator it = target_frames_.begin(); + typename V_string::iterator end = target_frames_.end(); + for (; it != end; ++it) { + const std::string & target = *it; + if (!bc_.canTransform(target, frame_id, tf2::timeFromSec(stamp.nanoseconds() / 1e+9))) { + can_transform = false; + break; + } + + if (time_tolerance_.nanoseconds()) { + if (!bc_.canTransform(target, frame_id, + tf2::timeFromSec((stamp + time_tolerance_).nanoseconds() / 1e+9))) + { + can_transform = false; + break; + } + } + } + } else { + can_transform = false; + } + + // We will be mutating messages now, require unique lock + std::unique_lock lock(messages_mutex_); + if (can_transform) { + TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", + frame_id.c_str(), (stamp.nanoseconds() / 1e+9), message_count_ - 1); + + ++successful_transform_count_; + messageReady(info.event); + } else { + ++dropped_message_count_; + + TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", + frame_id.c_str(), (stamp.nanoseconds() / 1e+9), message_count_ - 1); + messageDropped(info.event, filter_failure_reasons::Unknown); + } + + messages_.erase(msg_it); + --message_count_; + } + + /** + * \brief Callback that happens when we receive a message on the message topic + */ + void incomingMessage(const message_filters::MessageEvent & evt) + { + add(evt); + } + + void checkFailures() + { + if (!next_failure_warning_.nanoseconds()) { + next_failure_warning_ = rclcpp::Clock::now() + rclcpp::Duration(15, 0); + } + + if (rclcpp::Clock::now() >= next_failure_warning_) { + if (incoming_message_count_ - message_count_ == 0) { + return; + } + + double dropped_pct = static_cast(dropped_message_count_) / + static_cast(incoming_message_count_ - message_count_); + if (dropped_pct > 0.95) { + TF2_ROS_MESSAGEFILTER_WARN( + "Dropped %.2f%% of messages so far." + "Please turn the [%s.message_notifier] " + "rosconsole logger to DEBUG for more information.", dropped_pct * 100, + "tf2_ros_message_filter"); + next_failure_warning_ = rclcpp::Clock::now() + rclcpp::Duration(60, 0); + + if (static_cast(failed_out_the_back_count_) / + static_cast(dropped_message_count_) > 0.5) + { + TF2_ROS_MESSAGEFILTER_WARN( + " The majority of dropped messages were " + "due to messages growing older than the TF cache time. " + "The last message's timestamp was: %f, and the last frame_id was: %s", + (last_out_the_back_stamp_.nanoseconds() / 1e+9), last_out_the_back_frame_.c_str()); + } + } + } + } + + // TODO(clalancette): reenable this once we have underlying support for callback queues +#if 0 + struct CBQueueCallback : public ros::CallbackInterface + { + CBQueueCallback( + MessageFilter * filter, const MEvent & event, bool success, + FilterFailureReason reason) + : event_(event), + filter_(filter), + reason_(reason), + success_(success) + {} + + + virtual CallResult call() + { + if (success_) { + filter_->signalMessage(event_); + } else { + filter_->signalFailure(event_, reason_); + } + + return Success; + } + +private: + MEvent event_; + MessageFilter * filter_; + FilterFailureReason reason_; + bool success_; + }; +#endif + + void messageDropped(const MEvent & evt, FilterFailureReason reason) + { + // TODO(clalancette): reenable this once we have underlying support for callback queues +#if 0 + if (callback_queue_) { + ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason)); + callback_queue_->addCallback(cb, (uint64_t)this); + } else { + signalFailure(evt, reason); + } +#endif + signalFailure(evt, reason); + } + + void messageReady(const MEvent & evt) + { + // TODO(clalancette): reenable this once we have underlying support for callback queues +#if 0 + if (callback_queue_) { + ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, + filter_failure_reasons::Unknown)); + callback_queue_->addCallback(cb, (uint64_t)this); + } else { + this->signalMessage(evt); + } +#endif + this->signalMessage(evt); + } + + void signalFailure(const MEvent & evt, FilterFailureReason reason) + { + namespace mt = message_filters::message_traits; + const MConstPtr & message = evt.getMessage(); + std::string frame_id = stripSlash(mt::FrameId::value(*message)); + rclcpp::Time stamp = mt::TimeStamp::value(*message); + // RCLCPP_INFO(node_->get_logger(), "[%s] Drop message: frame'%s' at + // time %.3f for reason(%d)", + // __func__, frame_id.c_str(), (stamp.nanoseconds()/1e+9), reason); + } + + static std::string stripSlash(const std::string & in) + { + if (!in.empty() && (in[0] == '/')) { + std::string out = in; + out.erase(0, 1); + return out; + } + + return in; + } + + const rclcpp::Node::SharedPtr node_; + /// < The Transformer used to determine if transformation data is available + tf2::BufferCore & bc_; + /// < The frames we need to be able to transform to before a message is ready + V_string target_frames_; + std::string target_frames_string_; + /// < A mutex to protect access to the target_frames_ list and target_frames_string. + std::mutex target_frames_mutex_; + /// < The maximum number of messages we queue up + uint32_t queue_size_; + tf2::TransformableCallbackHandle callback_handle_; + + typedef std::vector V_TransformableRequestHandle; + struct MessageInfo + { + MessageInfo() + : success_count(0) {} + + MEvent event; + V_TransformableRequestHandle handles; + uint64_t success_count; + }; + typedef std::list L_MessageInfo; + L_MessageInfo messages_; + + /// < The number of messages in the list. + /// Used because \.size() may have linear cost + uint64_t message_count_; + /// < The mutex used for locking message list operations + std::mutex messages_mutex_; + uint64_t expected_success_count_; + + bool warned_about_empty_frame_id_; + + uint64_t successful_transform_count_; + uint64_t failed_out_the_back_count_; + uint64_t transform_message_count_; + uint64_t incoming_message_count_; + uint64_t dropped_message_count_; + + rclcpp::Time last_out_the_back_stamp_; + std::string last_out_the_back_frame_; + + rclcpp::Time next_failure_warning_; + + /// < Provide additional tolerance on time + /// for messages which are stamped but can have associated duration + rclcpp::Duration time_tolerance_ = rclcpp::Duration(0, 0); + + message_filters::Connection message_connection_; + message_filters::Connection message_connection_failure; +}; + +} // namespace tf2_ros + +#endif // TF2_ROS__MESSAGE_FILTER_HPP_ NOLINT diff --git a/include/laser_assembler/point_cloud_conversion.hpp b/include/laser_assembler/point_cloud_conversion.hpp new file mode 100644 index 0000000..721c28d --- /dev/null +++ b/include/laser_assembler/point_cloud_conversion.hpp @@ -0,0 +1,174 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ // NOLINT +#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ // NOLINT + +#include +#include "sensor_msgs/msg/point_cloud.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +// #include +#include "point_field_conversion.hpp" + +/** + * \brief Convert between the old (sensor_msgs::msg::PointCloud) and the new + * (sensor_msgs::msg::PointCloud2) format. \author Radu Bogdan Rusu + */ +namespace sensor_msgs +{ +////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Get the index of a specified field (i.e., dimension/channel) + * \param points the the point cloud message + * \param field_name the string defining the field name + */ +static inline int +getPointCloud2FieldIndex( + const sensor_msgs::msg::PointCloud2 & cloud, + const std::string & field_name) +{ + // Get the index we need + for (size_t d = 0; d < cloud.fields.size(); ++d) { + if (cloud.fields[d].name == field_name) { + return d; + } + } + return -1; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Convert a sensor_msgs::msg::PointCloud message to a + * sensor_msgs::msg::PointCloud2 message. \param input the message in the + * sensor_msgs::msg::PointCloud format \param output the resultant message in + * the sensor_msgs::msg::PointCloud2 format + */ +static inline bool +convertPointCloudToPointCloud2( + const sensor_msgs::msg::PointCloud & input, + sensor_msgs::msg::PointCloud2 & output) +{ + output.header = input.header; + output.width = input.points.size(); + output.height = 1; + output.fields.resize(3 + input.channels.size()); + // Convert x/y/z to fields + output.fields[0].name = "x"; + output.fields[1].name = "y"; + output.fields[2].name = "z"; + int offset = 0; + // All offsets are *4, as all field data types are float32 + for (size_t d = 0; d < output.fields.size(); ++d, offset += 4) { + output.fields[d].offset = offset; + output.fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; + output.fields[d].count = 1; + } + output.point_step = offset; + output.row_step = output.point_step * output.width; + // Convert the remaining of the channels to fields + for (size_t d = 0; d < input.channels.size(); ++d) { + output.fields[3 + d].name = input.channels[d].name; + } + output.data.resize(input.points.size() * output.point_step); + output.is_bigendian = false; // @todo ? + output.is_dense = false; + + // Copy the data points + for (size_t cp = 0; cp < input.points.size(); ++cp) { + memcpy(&output.data[cp * output.point_step + output.fields[0].offset], + &input.points[cp].x, sizeof(float)); + memcpy(&output.data[cp * output.point_step + output.fields[1].offset], + &input.points[cp].y, sizeof(float)); + memcpy(&output.data[cp * output.point_step + output.fields[2].offset], + &input.points[cp].z, sizeof(float)); + for (size_t d = 0; d < input.channels.size(); ++d) { + if (input.channels[d].values.size() == input.points.size()) { + memcpy( + &output.data[cp * output.point_step + output.fields[3 + d].offset], + &input.channels[d].values[cp], sizeof(float)); + } + } + } + return true; +} + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +/** \brief Convert a sensor_msgs::msg::PointCloud2 message to a + * sensor_msgs::msg::PointCloud message. \param input the message in the + * sensor_msgs::msg::PointCloud2 format \param output the resultant message in + * the sensor_msgs::msg::PointCloud format + */ +static inline bool +convertPointCloud2ToPointCloud( + const sensor_msgs::msg::PointCloud2 & input, + sensor_msgs::msg::PointCloud & output) +{ + output.header = input.header; + output.points.resize(input.width * input.height); + output.channels.resize(input.fields.size() - 3); + // Get the x/y/z field offsets + int x_idx = getPointCloud2FieldIndex(input, "x"); + int y_idx = getPointCloud2FieldIndex(input, "y"); + int z_idx = getPointCloud2FieldIndex(input, "z"); + if (x_idx == -1 || y_idx == -1 || z_idx == -1) { + return false; + } + int x_offset = input.fields[x_idx].offset; + int y_offset = input.fields[y_idx].offset; + int z_offset = input.fields[z_idx].offset; + uint8_t x_datatype = input.fields[x_idx].datatype; + uint8_t y_datatype = input.fields[y_idx].datatype; + uint8_t z_datatype = input.fields[z_idx].datatype; + + // Convert the fields to channels + int cur_c = 0; + for (size_t d = 0; d < input.fields.size(); ++d) { + if (reinterpret_cast(input.fields[d].offset) == x_offset || + reinterpret_cast(input.fields[d].offset) == y_offset || + reinterpret_cast(input.fields[d].offset) == z_offset) + { + continue; + } + output.channels[cur_c].name = input.fields[d].name; + output.channels[cur_c].values.resize(output.points.size()); + cur_c++; + } + + // Copy the data points + for (size_t cp = 0; cp < output.points.size(); ++cp) { + // Copy x/y/z + output.points[cp].x = sensor_msgs::readPointCloud2BufferValue( + &input.data[cp * input.point_step + x_offset], x_datatype); + output.points[cp].y = sensor_msgs::readPointCloud2BufferValue( + &input.data[cp * input.point_step + y_offset], y_datatype); + output.points[cp].z = sensor_msgs::readPointCloud2BufferValue( + &input.data[cp * input.point_step + z_offset], z_datatype); + // Copy the rest of the data + int cur_c = 0; + for (size_t d = 0; d < input.fields.size(); ++d) { + if (reinterpret_cast(input.fields[d].offset) == x_offset || + reinterpret_cast(input.fields[d].offset) == y_offset || + reinterpret_cast(input.fields[d].offset) == z_offset) + { + continue; + } + output.channels[cur_c++].values[cp] = + sensor_msgs::readPointCloud2BufferValue( + &input.data[cp * input.point_step + input.fields[d].offset], + input.fields[d].datatype); + } + } + return true; +} +} // namespace sensor_msgs +#endif // SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ // NOLINT diff --git a/include/laser_assembler/point_field_conversion.hpp b/include/laser_assembler/point_field_conversion.hpp new file mode 100644 index 0000000..c34d0eb --- /dev/null +++ b/include/laser_assembler/point_field_conversion.hpp @@ -0,0 +1,244 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. + +// point_field_conversion.h +// +// Created on: 16.07.2015 +// Authors: Sebastian Pütz + +#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ // NOLINT +#define SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ // NOLINT + +#include +/** + * \brief This file provides a type to enum mapping for the different + * PointField types and methods to read and write in + * a PointCloud2 buffer for the different PointField types. + * \author Sebastian Pütz + */ +namespace sensor_msgs +{ +/*! + * \Enum to type mapping. + */ +template +struct pointFieldTypeAsType {}; +template<> +struct pointFieldTypeAsType +{ + typedef int8_t type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef uint8_t type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef int16_t type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef uint16_t type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef int32_t type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef uint32_t type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef float type; +}; +template<> +struct pointFieldTypeAsType +{ + typedef double type; +}; + +/*! + * \Type to enum mapping. + */ +template +struct typeAsPointFieldType {}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::INT8; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::UINT8; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::INT16; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::UINT16; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::INT32; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::UINT32; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::FLOAT32; +}; +template<> +struct typeAsPointFieldType +{ + static const uint8_t value = sensor_msgs::msg::PointField::FLOAT64; +}; + +/*! + * \Converts a value at the given pointer position, interpreted as the datatype + * specified by the given template argument point_field_type, to the given + * template type T and returns it. + * \param data_ptr pointer into the point cloud 2 buffer + * \tparam point_field_type sensor_msgs::msg::PointField datatype value + * \tparam T return type + */ +template +inline T readPointCloud2BufferValue(const unsigned char * data_ptr) +{ + typedef typename pointFieldTypeAsType::type type; + return static_cast(*(reinterpret_cast(data_ptr))); +} + +/*! + * \Converts a value at the given pointer position interpreted as the datatype + * specified by the given datatype parameter to the given template type and + * returns it. \param data_ptr pointer into the point cloud 2 buffer \param + * datatype sensor_msgs::msg::PointField datatype value \tparam T return type + */ +template +inline T readPointCloud2BufferValue( + const unsigned char * data_ptr, + const unsigned char datatype) +{ + switch (datatype) { + case sensor_msgs::msg::PointField::INT8: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::UINT8: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::INT16: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::UINT16: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::INT32: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::UINT32: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::FLOAT32: + return readPointCloud2BufferValue( + data_ptr); + case sensor_msgs::msg::PointField::FLOAT64: + return readPointCloud2BufferValue( + data_ptr); + } + // This should never be reached, but return statement added to avoid compiler + // warning. (#84) + return T(); +} + +/*! + * \Inserts a given value at the given point position interpreted as the + * datatype specified by the template argument point_field_type. \param data_ptr + * pointer into the point cloud 2 buffer \param value the value to + * insert \tparam point_field_type sensor_msgs::msg::PointField datatype value + * \tparam T type of the value to insert + */ +template +inline void writePointCloud2BufferValue(unsigned char * data_ptr, T value) +{ + typedef typename pointFieldTypeAsType::type type; + *(reinterpret_cast(data_ptr)) = static_cast(value); +} + +/*! + * \Inserts a given value at the given point position interpreted as the + * datatype specified by the given datatype parameter. \param data_ptr pointer + * into the point cloud 2 buffer \param datatype sensor_msgs::msg::PointField + * datatype value \param value the value to insert \tparam T type + * of the value to insert + */ +template +inline void writePointCloud2BufferValue( + unsigned char * data_ptr, + const unsigned char datatype, T value) +{ + switch (datatype) { + case sensor_msgs::msg::PointField::INT8: + writePointCloud2BufferValue(data_ptr, + value); + break; + case sensor_msgs::msg::PointField::UINT8: + writePointCloud2BufferValue( + data_ptr, value); + break; + case sensor_msgs::msg::PointField::INT16: + writePointCloud2BufferValue( + data_ptr, value); + break; + case sensor_msgs::msg::PointField::UINT16: + writePointCloud2BufferValue( + data_ptr, value); + break; + case sensor_msgs::msg::PointField::INT32: + writePointCloud2BufferValue( + data_ptr, value); + break; + case sensor_msgs::msg::PointField::UINT32: + writePointCloud2BufferValue( + data_ptr, value); + break; + case sensor_msgs::msg::PointField::FLOAT32: + writePointCloud2BufferValue( + data_ptr, value); + break; + case sensor_msgs::msg::PointField::FLOAT64: + writePointCloud2BufferValue( + data_ptr, value); + break; + } +} +} // namespace sensor_msgs + +#endif // SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ // NOLINT diff --git a/launch/test_laser_assembler.launch.py b/launch/test_laser_assembler.launch.py new file mode 100644 index 0000000..2251ed8 --- /dev/null +++ b/launch/test_laser_assembler.launch.py @@ -0,0 +1,67 @@ +#!/usr/bin/python3.6 +# +# Copyright 2018 Open Source Robotics Foundation, Inc. +# +# 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. + +import os +import pathlib + +from launch import LaunchDescription +import launch.actions +from launch.substitutions import EnvironmentVariable +import launch_ros.actions + + +def generate_launch_description(): + parameters_file_dir = pathlib.Path(__file__).resolve().parent + parameters_file_path = parameters_file_dir / 'test_laser_assembler.yaml' + os.environ['FILE_PATH'] = str(parameters_file_dir) + os.environ['TOPIC_NAME'] = 'scan' + topic_prefix = 'dummy_' + + dummy_scan_producer = launch_ros.actions.Node( + package='laser_assembler', node_executable='dummy_scan_producer', output='screen') + + laser_scan_assembler = launch_ros.actions.Node( + package='laser_assembler', node_executable='laser_scan_assembler', + output='screen', + remappings=[ + ('scan', 'dummy_scan'), + (EnvironmentVariable(name='TOPIC_NAME'), [ + topic_prefix, EnvironmentVariable(name='TOPIC_NAME')]) + ], + parameters=[ + parameters_file_path, + str(parameters_file_path), + [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_laser_assembler.yaml'], + ]) + + test_laser_assembler = launch_ros.actions.Node( + package='laser_assembler', node_executable='test_assembler', + output='screen' + ) + + return LaunchDescription([ + launch.actions.DeclareLaunchArgument( + 'output_final_position', + default_value='false'), + dummy_scan_producer, + laser_scan_assembler, + test_laser_assembler, + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=test_laser_assembler, + on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + )), + ]) diff --git a/launch/test_laser_assembler.yaml b/launch/test_laser_assembler.yaml new file mode 100644 index 0000000..1b777df --- /dev/null +++ b/launch/test_laser_assembler.yaml @@ -0,0 +1,7 @@ +# +laser_scan_assembler: + ros__parameters: + tf_cache_time_secs: 10.0 + max_scans: 400 + ignore_laser_skew: false + fixed_frame: dummy_base_link diff --git a/package.xml b/package.xml index ba0b295..a1a19f0 100644 --- a/package.xml +++ b/package.xml @@ -1,39 +1,52 @@ - + + laser_assembler Provides nodes to assemble point clouds from either LaserScan or PointCloud messages - 1.7.6 + 1.7.7 Vijay Pradeep Jonathan Binney BSD http://ros.org/wiki/laser_assembler - - catkin - - message_generation + ament_cmake + sensor_msgs + builtin_interfaces + rosidl_default_generators sensor_msgs message_filters - tf - roscpp - rostest + tf2 + tf2_ros + rclcpp filters + rostest + message_filters laser_geometry - pluginlib - - message_runtime - sensor_msgs - message_filters - roscpp - tf - filters - laser_geometry - pluginlib + builtin_interfaces + + sensor_msgs + message_filters + rclcpp + tf2 + tf2_ros + message_filters + filters + laser_geometry + builtin_interfaces + rosidl_default_runtime + sensor_msgs + + ament_lint_auto + ament_lint_common + builtin_interfaces + + rosidl_interface_packages + ament_cmake diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index 6d76e78..f12f38c 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -1,114 +1,114 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - - -#include "laser_geometry/laser_geometry.h" -#include "sensor_msgs/LaserScan.h" -#include "laser_assembler/base_assembler.h" -#include "filters/filter_chain.h" - -using namespace laser_geometry; -using namespace std ; +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 +#include +#include +#include +#include +#include "laser_geometry/laser_geometry.hpp" +#include "filters/filter_chain.hpp" +#include "laser_assembler/base_assembler.hpp" +#include "rclcpp/time.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +#define TIME rclcpp::Time namespace laser_assembler { /** - * \brief Maintains a history of laser scans and generates a point cloud upon request + * \brief Maintains a history of laser scans and generates a point cloud upon + * request */ -class LaserScanAssembler : public BaseAssembler +class LaserScanAssembler : public BaseAssembler { public: - LaserScanAssembler() : BaseAssembler("max_scans"), filter_chain_("sensor_msgs::LaserScan") + explicit LaserScanAssembler(const rclcpp::Node::SharedPtr & node_) + : BaseAssembler("max_scans", node_), + filter_chain_("sensor_msgs::msg::LaserScan") { // ***** Set Laser Projection Method ***** - private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true); + n_->get_parameter_or("ignore_laser_skew", ignore_laser_skew_, + true); // configure the filter chain from the parameter server - filter_chain_.configure("filters", private_ns_); + filter_chain_.configure("filters", node_); - // Have different callbacks, depending on whether or not we want to ignore laser skews. - if (ignore_laser_skew_) + // Have different callbacks, depending on whether or not we want to ignore + // laser skews. + if (ignore_laser_skew_) { start("scan"); - else - { + } else { start(); - skew_scan_sub_ = n_.subscribe("scan", 10, &LaserScanAssembler::scanCallback, this); + skew_scan_sub_ = n_->create_subscription( + "scan", std::bind(&LaserScanAssembler::scanCallback, this, + std::placeholders::_1)); } } - ~LaserScanAssembler() - { - - } + ~LaserScanAssembler() {} - unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::LaserScan & scan) { - return (scan.ranges.size ()); + return scan.ranges.size(); } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out) + void ConvertToCloud( + const std::string & fixed_frame_id, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) { // apply filters on laser scan - filter_chain_.update (scan_in, scan_filtered_); + filter_chain_.update(scan_in, scan_filtered_); + + int mask = laser_geometry::channel_option::Intensity + + laser_geometry::channel_option::Distance + + laser_geometry::channel_option::Index + + laser_geometry::channel_option::Timestamp; + + unsigned int n_pts = scan_in.ranges.size(); - // convert laser scan to point cloud - if (ignore_laser_skew_) // Do it the fast (approximate) way - { + // TODO(vandana) ignore_laser_skew_ this case is not handled right now, as there is + // no transformPointCloud support for PointCloud in bouncy. + if (ignore_laser_skew_) { // Do it the fast (approximate) way projector_.projectLaser(scan_filtered_, cloud_out); - if (cloud_out.header.frame_id != fixed_frame_id) - tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); - } - else // Do it the slower (more accurate) way - { + if (cloud_out.header.frame_id != fixed_frame_id) { + // TODO(vandana) transform PointCloud + // tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); + } + } else { // Do it the slower (more accurate) way int mask = laser_geometry::channel_option::Intensity + - laser_geometry::channel_option::Distance + - laser_geometry::channel_option::Index + - laser_geometry::channel_option::Timestamp; - projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask); + laser_geometry::channel_option::Distance + + laser_geometry::channel_option::Index + + laser_geometry::channel_option::Timestamp; + projector_.transformLaserScanToPointCloud(fixed_frame_id, scan_filtered_, + cloud_out, tfBuffer, mask); } - return; } - void scanCallback(const sensor_msgs::LaserScanConstPtr& laser_scan) + void + scanCallback(std::shared_ptr laser_scan) { - if (!ignore_laser_skew_) - { - ros::Duration cur_tolerance = ros::Duration(laser_scan->time_increment * laser_scan->ranges.size()); - if (cur_tolerance > max_tolerance_) - { - ROS_DEBUG("Upping tf tolerance from [%.4fs] to [%.4fs]", max_tolerance_.toSec(), cur_tolerance.toSec()); + if (!ignore_laser_skew_) { + rclcpp::Duration cur_tolerance = rclcpp::Duration( + laser_scan->time_increment * laser_scan->ranges.size()); + if (cur_tolerance > max_tolerance_) { + RCLCPP_DEBUG(g_logger, "Upping tf tolerance from [%.4fs] to [%.4fs]", + max_tolerance_.nanoseconds() / 1e+9, + cur_tolerance.nanoseconds() / 1e+9); assert(tf_filter_); tf_filter_->setTolerance(cur_tolerance); max_tolerance_ = cur_tolerance; @@ -121,23 +121,23 @@ class LaserScanAssembler : public BaseAssembler bool ignore_laser_skew_; laser_geometry::LaserProjection projector_; - ros::Subscriber skew_scan_sub_; - ros::Duration max_tolerance_; // The longest tolerance we've needed on a scan so far - - filters::FilterChain filter_chain_; - mutable sensor_msgs::LaserScan scan_filtered_; + rclcpp::Subscription::SharedPtr skew_scan_sub_; + rclcpp::Duration max_tolerance_ = rclcpp::Duration( + 0, 0); // The longest tolerance we've needed on a scan so far + filters::FilterChain filter_chain_; + mutable sensor_msgs::msg::LaserScan scan_filtered_ /*scan_filtered_*/; }; -} - -using namespace laser_assembler ; +} // namespace laser_assembler -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "laser_scan_assembler"); - LaserScanAssembler pc_assembler; - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("laser_scan_assembler"); + g_logger = node->get_logger(); + laser_assembler::LaserScanAssembler pc_assembler(node); + rclcpp::spin(node); return 0; } diff --git a/src/laser_scan_assembler_srv.cpp b/src/laser_scan_assembler_srv.cpp index 5137e04..7ed5bc0 100644 --- a/src/laser_scan_assembler_srv.cpp +++ b/src/laser_scan_assembler_srv.cpp @@ -1,122 +1,103 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - - -#include "laser_geometry/laser_geometry.h" -#include "sensor_msgs/LaserScan.h" -#include "laser_assembler/base_assembler_srv.h" +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 #include "filters/filter_chain.h" - -using namespace laser_geometry; -using namespace std ; +#include "geometry_msgs/msg/transform_stamped.hpp"; +#include "laser_assembler/base_assembler_srv.hpp" +#include "laser_geometry/laser_geometry.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" namespace laser_assembler { - /** - * \brief Maintains a history of laser scans and generates a point cloud upon request - * \section params ROS Parameters + * \brief Maintains a history of laser scans and generates a point cloud upon + * request \section params ROS Parameters * - (Several params are inherited from laser_assembler::BaseAssemblerSrv) * - \b "~ignore_laser_skew" (bool) - Specifies the method to project laser data - * - true -> Account for laser skew, and compute the transform for each laser point (This is currently really slow!) - * - false-> Don't account for laser skew, and use 1 transform per scanline. (This might have a little error when moving fast) - * \section services ROS Services + * - true -> Account for laser skew, and compute the transform for each laser + * point (This is currently really slow!) + * - false-> Don't account for laser skew, and use 1 transform per scanline. + * (This might have a little error when moving fast) \section services ROS + * Services * - "~build_cloud" - Inhertited from laser_assembler::BaseAssemblerSrv */ -class LaserScanAssemblerSrv : public BaseAssemblerSrv +class LaserScanAssemblerSrv + : public BaseAssemblerSrv { public: - LaserScanAssemblerSrv() : filter_chain_("sensor_msgs::LaserScan") + LaserScanAssemblerSrv() + : filter_chain_("sensor_msgs::msg::LaserScan") { // ***** Set Laser Projection Method ***** - private_ns_.param("ignore_laser_skew", ignore_laser_skew_, true); + private_ns_->get_parameter_or("ignore_laser_skew", ignore_laser_skew_, + true); // configure the filter chain from the parameter server filter_chain_.configure("filters", private_ns_); } - ~LaserScanAssemblerSrv() - { + ~LaserScanAssemblerSrv() {} - } - - unsigned int GetPointsInScan(const sensor_msgs::LaserScan& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::LaserScan & scan) { return scan.ranges.size(); } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::LaserScan& scan_in, sensor_msgs::PointCloud& cloud_out) + void ConvertToCloud( + const string & fixed_frame_id, + const sensor_msgs::msg::LaserScan & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) { // apply filters on laser scan - filter_chain_.update (scan_in, scan_filtered_); + filter_chain_.update(scan_in, scan_filtered_); // convert laser scan to point cloud - if (ignore_laser_skew_) // Do it the fast (approximate) way - { + if (ignore_laser_skew_) { // Do it the fast (approximate) way projector_.projectLaser(scan_filtered_, cloud_out); - if (cloud_out.header.frame_id != fixed_frame_id) + if (cloud_out.header.frame_id != fixed_frame_id) { tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); + } + } else { // Do it the slower (more accurate) way + int mask = laser_geometry::channel_option::Intensity + + laser_geometry::channel_option::Distance + + laser_geometry::channel_option::Index + + laser_geometry::channel_option::Timestamp; + projector_.transformLaserScanToPointCloud(fixed_frame_id, scan_filtered_, + cloud_out, *tf_, mask); } - else // Do it the slower (more accurate) way - { - int mask = laser_geometry::channel_option::Intensity + laser_geometry::channel_option::Distance + laser_geometry::channel_option::Index + laser_geometry::channel_option::Timestamp; - projector_.transformLaserScanToPointCloud (fixed_frame_id, scan_filtered_, cloud_out, *tf_, mask); - } - return; } private: bool ignore_laser_skew_; laser_geometry::LaserProjection projector_; - filters::FilterChain filter_chain_; - mutable sensor_msgs::LaserScan scan_filtered_; - + filters::FilterChain filter_chain_; + mutable sensor_msgs::msg::LaserScan scan_filtered_; }; -} - -using namespace laser_assembler ; +} // namespace laser_assembler -int main(int argc, char **argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "laser_scan_assembler"); ros::NodeHandle n; - ROS_WARN("The laser_scan_assembler_srv is deprecated. Please switch to " - "using the laser_scan_assembler. Documentation is available at " - "http://www.ros.org/wiki/laser_assembler"); - LaserScanAssemblerSrv pc_assembler; + RCLCPP_WARN(n_->get_logger(), "The laser_scan_assembler_srv is deprecated. Please switch to " + "using the laser_scan_assembler. Documentation is available at " + "http://www.ros.org/wiki/laser_assembler"); + laser_assembler::LaserScanAssemblerSrv pc_assembler; pc_assembler.start(); ros::spin(); diff --git a/src/merge_clouds.cpp b/src/merge_clouds.cpp index bb84278..e18b585 100644 --- a/src/merge_clouds.cpp +++ b/src/merge_clouds.cpp @@ -1,36 +1,16 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * 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 the Willow Garage 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. - *********************************************************************/ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. /** \author Ioan Sucan */ @@ -40,64 +20,64 @@ potentially from different sensors **/ - -#include -#include -#include -#include - -#include -#include -#include - -#include +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "laser_assembler/message_filter.hpp" +#include "tf2_ros/transform_listener.h" +#include "message_filters/subscriber.h" class MergeClouds { public: - - MergeClouds(void) : - sub1_(nh_, "cloud_in1", 1), - sub2_(nh_, "cloud_in1", 1) + MergeClouds(void) + : sub1_(nh_, "cloud_in1", 1), sub2_(nh_, "cloud_in1", 1) { - cloudOut_ = nh_.advertise("cloud_out", 1); + cloudOut_ = nh_.advertise("cloud_out", 1); nh_.param("~output_frame", output_frame_, std::string()); nh_.param("~max_frequency", max_freq_, 0.0); newCloud1_ = newCloud2_ = false; - if (output_frame_.empty()) + if (output_frame_.empty()) { ROS_ERROR("No output frame specified for merging pointclouds"); + } // make sure we don't publish too fast - if (max_freq_ > 1000.0 || max_freq_ < 0.0) + if (max_freq_ > 1000.0 || max_freq_ < 0.0) { max_freq_ = 0.0; + } - if (max_freq_ > 0.0) - { - timer_ = nh_.createTimer(ros::Duration(1.0/max_freq_), boost::bind(&MergeClouds::onTimer, this, _1)); + if (max_freq_ > 0.0) { + timer_ = nh_.createTimer(rclcpp::Duration(1.0 / max_freq_), + std::bind(&MergeClouds::onTimer, this, _1)); haveTimer_ = true; - } - else + } else { haveTimer_ = false; + } - tf_filter1_.reset(new tf::MessageFilter(sub1_, tf_, output_frame_, 1)); - tf_filter2_.reset(new tf::MessageFilter(sub2_, tf_, output_frame_, 1)); + tf_filter1_.reset(new tf2_ros::MessageFilter( + sub1_, tf_, output_frame_, 1)); + tf_filter2_.reset(new tf2_ros::MessageFilter( + sub2_, tf_, output_frame_, 1)); - tf_filter1_->registerCallback(boost::bind(&MergeClouds::receiveCloud1, this, _1)); - tf_filter2_->registerCallback(boost::bind(&MergeClouds::receiveCloud2, this, _1)); + tf_filter1_->registerCallback( + std::bind(&MergeClouds::receiveCloud1, this, _1)); + tf_filter2_->registerCallback( + std::bind(&MergeClouds::receiveCloud2, this, _1)); } - ~MergeClouds(void) - { - - } + ~MergeClouds(void) {} private: - - void onTimer(const ros::TimerEvent &e) + void onTimer(const ros::TimerEvent & e) { - if (newCloud1_ && newCloud2_) + if (newCloud1_ && newCloud2_) { publishClouds(); + } } void publishClouds(void) @@ -108,33 +88,42 @@ class MergeClouds newCloud1_ = false; newCloud2_ = false; - sensor_msgs::PointCloud out; - if (cloud1_.header.stamp > cloud2_.header.stamp) + sensor_msgs::msg::PointCloud out; + if (cloud1_.header.stamp > cloud2_.header.stamp) { out.header = cloud1_.header; - else + } else { out.header = cloud2_.header; + } out.points.resize(cloud1_.points.size() + cloud2_.points.size()); // copy points std::copy(cloud1_.points.begin(), cloud1_.points.end(), out.points.begin()); - std::copy(cloud2_.points.begin(), cloud2_.points.end(), out.points.begin() + cloud1_.points.size()); + std::copy(cloud2_.points.begin(), cloud2_.points.end(), + out.points.begin() + cloud1_.points.size()); // copy common channels - for (unsigned int i = 0 ; i < cloud1_.channels.size() ; ++i) - for (unsigned int j = 0 ; j < cloud2_.channels.size() ; ++j) - if (cloud1_.channels[i].name == cloud2_.channels[j].name) - { - ROS_ASSERT(cloud1_.channels[i].values.size() == cloud1_.points.size()); - ROS_ASSERT(cloud2_.channels[j].values.size() == cloud2_.points.size()); + for (unsigned int i = 0; i < cloud1_.channels.size(); ++i) { + for (unsigned int j = 0; j < cloud2_.channels.size(); ++j) { + if (cloud1_.channels[i].name == cloud2_.channels[j].name) { + ASSERT_TRUE(cloud1_.channels[i].values.size() == cloud1_.points.size()); + ASSERT_TRUE(cloud2_.channels[j].values.size() == cloud2_.points.size()); unsigned int oc = out.channels.size(); out.channels.resize(oc + 1); out.channels[oc].name = cloud1_.channels[i].name; - out.channels[oc].values.resize(cloud1_.channels[i].values.size() + cloud2_.channels[j].values.size()); - std::copy(cloud1_.channels[i].values.begin(), cloud1_.channels[i].values.end(), out.channels[oc].values.begin()); - std::copy(cloud2_.channels[j].values.begin(), cloud2_.channels[j].values.end(), out.channels[oc].values.begin() + cloud1_.channels[i].values.size()); + out.channels[oc].values.resize(cloud1_.channels[i].values.size() + + cloud2_.channels[j].values.size()); + std::copy(cloud1_.channels[i].values.begin(), + cloud1_.channels[i].values.end(), + out.channels[oc].values.begin()); + std::copy(cloud2_.channels[j].values.begin(), + cloud2_.channels[j].values.end(), + out.channels[oc].values.begin() + + cloud1_.channels[i].values.size()); break; } + } + } lock1_.unlock(); lock2_.unlock(); @@ -142,67 +131,69 @@ class MergeClouds cloudOut_.publish(out); } - void receiveCloud1(const sensor_msgs::PointCloudConstPtr &cloud) + void receiveCloud1(const sensor_msgs::msg::PointCloudConstPtr & cloud) { lock1_.lock(); processCloud(cloud, cloud1_); lock1_.unlock(); newCloud1_ = true; - if (!haveTimer_ && newCloud2_) + if (!haveTimer_ && newCloud2_) { publishClouds(); + } } - void receiveCloud2(const sensor_msgs::PointCloudConstPtr &cloud) + void receiveCloud2(const sensor_msgs::msg::PointCloudConstPtr & cloud) { lock2_.lock(); processCloud(cloud, cloud2_); lock2_.unlock(); newCloud2_ = true; - if (!haveTimer_ && newCloud1_) + if (!haveTimer_ && newCloud1_) { publishClouds(); + } } - void processCloud(const sensor_msgs::PointCloudConstPtr &cloud, sensor_msgs::PointCloud &cloudOut) + void processCloud( + const sensor_msgs::msg::PointCloudConstPtr & cloud, + sensor_msgs::msg::PointCloud & cloudOut) { - if (output_frame_ != cloud->header.frame_id) + if (output_frame_ != cloud->header.frame_id) { tf_.transformPointCloud(output_frame_, *cloud, cloudOut); - else + } else { cloudOut = *cloud; + } } - ros::NodeHandle nh_; + ros::NodeHandle nh_; tf::TransformListener tf_; - ros::Timer timer_; - bool haveTimer_; - - ros::Publisher cloudOut_; - double max_freq_; - std::string output_frame_; + ros::Timer timer_; + bool haveTimer_; - message_filters::Subscriber sub1_; - message_filters::Subscriber sub2_; - boost::shared_ptr > tf_filter1_; - boost::shared_ptr > tf_filter2_; + ros::Publisher cloudOut_; + double max_freq_; + std::string output_frame_; - bool newCloud1_; - bool newCloud2_; - sensor_msgs::PointCloud cloud1_; - sensor_msgs::PointCloud cloud2_; - boost::mutex lock1_; - boost::mutex lock2_; + message_filters::Subscriber sub1_; + message_filters::Subscriber sub2_; + std::shared_ptr> tf_filter1_; + std::shared_ptr> tf_filter2_; + bool newCloud1_; + bool newCloud2_; + sensor_msgs::msg::PointCloud cloud1_; + sensor_msgs::msg::PointCloud cloud2_; + std::mutex lock1_; + std::mutex lock2_; }; - -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "merge_clouds", ros::init_options::AnonymousName); - - ROS_WARN("laser_assembler/merge_clouds is deprecated. You should instead be using 3dnav_pr2/merge_clouds"); - + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("merge_clouds"); MergeClouds mc; - ros::spin(); + mc.start("cloud"); + rclcpp::spin(node); return 0; } diff --git a/src/point_cloud2_assembler.cpp b/src/point_cloud2_assembler.cpp index 7f9f166..1b31112 100644 --- a/src/point_cloud2_assembler.cpp +++ b/src/point_cloud2_assembler.cpp @@ -1,92 +1,67 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - - -#include "laser_assembler/base_assembler.h" -#include - -using namespace std ; +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 +#include "laser_assembler/base_assembler.hpp" +// #include // TODO +#include "laser_assembler/point_cloud_conversion.hpp" namespace laser_assembler { /** - * \brief Maintains a history of incremental point clouds (usually from laser scans) and generates a point cloud upon request - * \todo Clean up the doxygen part of this header - * params + * \brief Maintains a history of incremental point clouds (usually from laser + * scans) and generates a point cloud upon request \todo Clean up the doxygen + * part of this header params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloud2Assembler : public BaseAssembler +class PointCloud2Assembler + : public BaseAssembler { public: - PointCloud2Assembler() : BaseAssembler("max_clouds") - { - - } + explicit PointCloud2Assembler(rclcpp::Node::SharedPtr node) + : BaseAssembler("max_clouds", node) {} - ~PointCloud2Assembler() - { - - } + ~PointCloud2Assembler() {} - unsigned int GetPointsInScan(const sensor_msgs::PointCloud2& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::PointCloud2 & scan) { - return (scan.width * scan.height); + return scan.width * scan.height; } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::PointCloud2& scan_in, sensor_msgs::PointCloud& cloud_out) + void ConvertToCloud( + const string & fixed_frame_id, + const sensor_msgs::msg::PointCloud2 & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) { - sensor_msgs::PointCloud cloud_in; + sensor_msgs::msg::PointCloud cloud_in; sensor_msgs::convertPointCloud2ToPointCloud(scan_in, cloud_in); - tf_->transformPointCloud(fixed_frame_id, cloud_in, cloud_out) ; - return ; + tf_->transformPointCloud(fixed_frame_id, cloud_in, cloud_out); } private: - }; -} - -using namespace laser_assembler ; +} // namespace laser_assembler -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "point_cloud_assembler"); - PointCloud2Assembler pc_assembler; + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("point_cloud_assembler"); + laser_assembler::PointCloud2Assembler pc_assembler(node); pc_assembler.start("cloud"); - ros::spin(); + rclcpp::spin(node); return 0; } diff --git a/src/point_cloud_assembler.cpp b/src/point_cloud_assembler.cpp index db20979..857e315 100644 --- a/src/point_cloud_assembler.cpp +++ b/src/point_cloud_assembler.cpp @@ -1,90 +1,62 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - - -#include "laser_assembler/base_assembler.h" - - -using namespace std ; +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 +#include "laser_assembler/base_assembler.hpp" namespace laser_assembler { /** - * \brief Maintains a history of incremental point clouds (usually from laser scans) and generates a point cloud upon request - * \todo Clean up the doxygen part of this header - * params + * \brief Maintains a history of incremental point clouds (usually from laser + * scans) and generates a point cloud upon request \todo Clean up the doxygen + * part of this header params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloudAssembler : public BaseAssembler +class PointCloudAssembler : public BaseAssembler { public: - PointCloudAssembler() : BaseAssembler("max_clouds") - { + explicit PointCloudAssembler(rclcpp::Node::SharedPtr node) + : BaseAssembler("max_clouds", node) {} - } + ~PointCloudAssembler() {} - ~PointCloudAssembler() + unsigned int GetPointsInScan(const sensor_msgs::msg::PointCloud & scan) { - + return scan.points.size(); } - unsigned int GetPointsInScan(const sensor_msgs::PointCloud& scan) + void ConvertToCloud( + const std::string & fixed_frame_id, + const sensor_msgs::msg::PointCloud & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) { - return (scan.points.size ()); - } - - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::PointCloud& scan_in, sensor_msgs::PointCloud& cloud_out) - { - tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out) ; - return ; + tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out); } private: - }; -} - -using namespace laser_assembler ; +} // namespace laser_assembler -int main(int argc, char **argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "point_cloud_assembler"); - PointCloudAssembler pc_assembler; + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("point_cloud_assembler"); + laser_assembler::PointCloudAssembler pc_assembler(node); pc_assembler.start("cloud"); - ros::spin(); + rclcpp::spin(node); return 0; } diff --git a/src/point_cloud_assembler_srv.cpp b/src/point_cloud_assembler_srv.cpp index dbbef7d..72725dd 100644 --- a/src/point_cloud_assembler_srv.cpp +++ b/src/point_cloud_assembler_srv.cpp @@ -1,94 +1,65 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ - - -#include "laser_assembler/base_assembler_srv.h" - - -using namespace std; +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 +#include "laser_assembler/base_assembler_srv.hpp" namespace laser_assembler { - /** * \brief Maintains a history of incremental point clouds (usually from laser scans) and generates a point cloud upon request * \todo Clean up the doxygen part of this header * params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloudAssemblerSrv : public BaseAssemblerSrv +class PointCloudAssemblerSrv : public BaseAssemblerSrv { public: PointCloudAssemblerSrv() { - } ~PointCloudAssemblerSrv() { - } - unsigned int GetPointsInScan(const sensor_msgs::PointCloud& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::PointCloud & scan) { - return scan.points.size() ; + return scan.points.size(); } - void ConvertToCloud(const string& fixed_frame_id, const sensor_msgs::PointCloud& scan_in, sensor_msgs::PointCloud& cloud_out) + void ConvertToCloud( + const string & fixed_frame_id, const sensor_msgs::PointCloud & scan_in, + sensor_msgs::PointCloud & cloud_out) { - tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out) ; - return ; + tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out); } private: - }; -} +} // namespace laser_assembler -using namespace laser_assembler ; - -int main(int argc, char **argv) +int main(int argc, char ** argv) { ros::init(argc, argv, "point_cloud_assembler"); ros::NodeHandle n; - ROS_WARN("The point_cloud_assembler_srv is deprecated. Please switch to " - "using the laser_scan_assembler. Documentation is available at " - "http://www.ros.org/wiki/laser_assembler"); - PointCloudAssemblerSrv pc_assembler; - pc_assembler.start() ; + RCLCPP_WARN(n_->get_logger(), "The point_cloud_assembler_srv is deprecated. Please switch to " + "using the laser_scan_assembler. Documentation is available at " + "http://www.ros.org/wiki/laser_assembler"); + laser_assembler::PointCloudAssemblerSrv pc_assembler; + pc_assembler.start(); ros::spin(); - return 0; } diff --git a/srv/AssembleScans.srv b/srv/AssembleScans.srv index b73949e..3c7b31b 100644 --- a/srv/AssembleScans.srv +++ b/srv/AssembleScans.srv @@ -1,7 +1,7 @@ # The time interval on which we want to aggregate scans -time begin +int64 begin # The end of the interval on which we want to assemble scans or clouds -time end +int64 end --- # The point cloud holding the assembled clouds or scans. # This cloud is in the frame specified by the ~fixed_frame node parameter. diff --git a/srv/AssembleScans2.srv b/srv/AssembleScans2.srv index 8071506..e2bafd1 100644 --- a/srv/AssembleScans2.srv +++ b/srv/AssembleScans2.srv @@ -1,7 +1,7 @@ # The time interval on which we want to aggregate scans -time begin +int64 begin # The end of the interval on which we want to assemble scans or clouds -time end +int64 end --- # The point cloud holding the assembled clouds or scans. # This cloud is in the frame specified by the ~fixed_frame node parameter. diff --git a/test/dummy_scan_producer.cpp b/test/dummy_scan_producer.cpp index 5c1e4c2..6c7da1d 100644 --- a/test/dummy_scan_producer.cpp +++ b/test/dummy_scan_producer.cpp @@ -1,62 +1,46 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. /* Author: Vijay Pradeep */ /** - * Generate dummy scans in order to not be dependent on bag files in order to run tests + * Generate dummy scans in order to not be dependent on bag files in order to + *run tests **/ -#include -#include -#include -#include +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "tf2/LinearMath/Transform.h" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/transform_broadcaster.h" -void runLoop() +void runLoop(rclcpp::Node::SharedPtr node_) { - ros::NodeHandle nh; + rclcpp::Rate loopRate(5); - ros::Publisher scan_pub = nh.advertise("dummy_scan", 100); - ros::Rate loop_rate(5); + auto scan_pub = + node_->create_publisher("dummy_scan", 100); // Configure the Transform broadcaster - tf::TransformBroadcaster broadcaster; - tf::Transform laser_transform(tf::Quaternion(0,0,0,1)); + tf2_ros::TransformBroadcaster broadcaster(node_); + tf2::Transform laser_transform(tf2::Quaternion(0, 0, 0, 1)); // Populate the dummy laser scan - sensor_msgs::LaserScan scan; - scan.header.frame_id = "/dummy_laser_link"; + sensor_msgs::msg::LaserScan scan; + scan.header.frame_id = "dummy_laser_link"; scan.angle_min = 0.0; scan.angle_max = 99.0; scan.angle_increment = 1.0; @@ -69,29 +53,34 @@ void runLoop() scan.ranges.resize(N); scan.intensities.resize(N); - for (unsigned int i=0; iget_logger(), "Publishing scan\n"); + scan_pub->publish(scan); + + geometry_msgs::msg::TransformStamped tf_transform; + tf_transform.header.stamp = rclcpp::Clock().now(); + tf_transform.header.frame_id = "dummy_laser_link"; + tf_transform.child_frame_id = "dummy_base_link"; + tf_transform.transform.rotation.w = 1.0; + broadcaster.sendTransform(tf_transform); + loopRate.sleep(); } } -int main(int argc, char** argv) +int main(int argc, char ** argv) { - ros::init(argc, argv, "scan_producer"); - ros::NodeHandle nh; - boost::thread run_thread(&runLoop); - ros::spin(); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("dummy_scan_producer"); + std::thread run_thread(&runLoop, node); + rclcpp::spin(node); run_thread.join(); return 0; } diff --git a/test/test_assembler.cpp b/test/test_assembler.cpp index bc760a8..d0d959e 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -1,143 +1,150 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* All rights reserved. -* -* 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 the Willow Garage 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. -*********************************************************************/ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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. /* Author: Vijay Pradeep */ #include -#include -#include -#include "sensor_msgs/PointCloud.h" -#include "sensor_msgs/LaserScan.h" -#include "laser_assembler/AssembleScans.h" -#include -#include +#include +#include +#include +#include "laser_assembler/srv/assemble_scans.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.h" +#include "gtest/gtest.h" -using namespace ros; -using namespace sensor_msgs; -using namespace std; - -static const string SERVICE_NAME = "assemble_scans"; +static const char SERVICE_NAME[] = "assemble_scans"; class TestAssembler : public testing::Test { public: + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr + client_; + + TestAssembler() {} - NodeHandle n_; + static void spinThread(rclcpp::Node::SharedPtr node_) {rclcpp::spin(node_);} void SetUp() { - ROS_INFO("Waiting for service [%s]", SERVICE_NAME.c_str()); - ros::service::waitForService(SERVICE_NAME); - ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + node_ = rclcpp::Node::make_shared("test_assembler"); + std::thread spin_thread(spinThread, node_); + spin_thread.detach(); + client_ = node_->create_client( + "assemble_scans"); + using namespace std::chrono_literals; + RCLCPP_INFO(node_->get_logger(), "Waiting for service [%s]", SERVICE_NAME); + if (!client_->wait_for_service(20s)) { + RCLCPP_ERROR(node_->get_logger(), "Service not available after waiting"); + return; + } + RCLCPP_INFO(node_->get_logger(), "Service assemble_scans started successfully"); received_something_ = false; got_cloud_ = false; - scan_sub_ = n_.subscribe("dummy_scan", 10, &TestAssembler::ScanCallback, (TestAssembler*)this); + + auto scan_callback = + [this](sensor_msgs::msg::LaserScan::ConstSharedPtr msg) -> void { + this->ScanCallback(msg); + }; + scan_sub_ = node_->create_subscription( + "dummy_scan", scan_callback); } - void ScanCallback(const LaserScanConstPtr& scan_msg) + void ScanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg) { - boost::mutex::scoped_lock lock(mutex_); - if (!received_something_) - { - // Store the time of this first scan. Will be needed when we make the service call + std::unique_lock lock(mutex_); + if (!received_something_) { + // Store the time of this first scan. Will be needed when we make the + // service call start_time_ = scan_msg->header.stamp; received_something_ = true; - } - else - { + } else { // Make the call to get a point cloud - laser_assembler::AssembleScans assemble_scans; - assemble_scans.request.begin = start_time_; - assemble_scans.request.end = scan_msg->header.stamp; - EXPECT_TRUE(ros::service::call(SERVICE_NAME, assemble_scans)); - if(assemble_scans.response.cloud.points.size() > 0) - { - ROS_INFO("Got a cloud with [%u] points. Saving the cloud", (uint32_t)(assemble_scans.response.cloud.points.size())); - cloud_ = assemble_scans.response.cloud; - got_cloud_ = true; - cloud_condition_.notify_all(); - } - else - ROS_INFO("Got an empty cloud. Going to try again on the next scan"); + auto request = std::make_shared< + laser_assembler::srv::AssembleScans::Request>(); + request->begin = start_time_.sec; + request->end = scan_msg->header.stamp.sec; + + // In order to wait for a response to arrive, we need to spin(). + // However, this function is already being called from within another + // spin(). Unfortunately, the current version of spin() is not recursive + // and so we cannot call spin() from within another spin(). Therefore, we + // cannot wait for a response to the request we just made here within this + // callback, because it was executed by some other spin function. The + // workaround for this is to give the async_send_request() method another + // argument which is a callback that gets executed once the future is + // ready. We then return from this callback so that the existing spin() + // function can continue and our callback will get called once the + // response is received. + using ServiceResponseFuture = rclcpp::Client< + laser_assembler::srv::AssembleScans>::SharedFuture; + + auto response_received_callback = [this](ServiceResponseFuture future) { + auto result = future.get(); + RCLCPP_INFO(node_->get_logger(), "Got result: [ %ld ]", + result.get()->cloud.points.size()); + if (result.get()->cloud.points.size() > 0) { + cloud_ = result.get()->cloud; + got_cloud_ = true; + cloud_condition_.notify_all(); + } else { + RCLCPP_INFO( + node_->get_logger(), "Got an empty cloud. Going to try again on the next scan"); + } + }; + + auto result = + client_->async_send_request(request, response_received_callback); } } protected: - ros::Subscriber scan_sub_; + rclcpp::Subscription::SharedPtr scan_sub_; + bool received_something_; - ros::Time start_time_; + + builtin_interfaces::msg::Time start_time_; + bool got_cloud_; - sensor_msgs::PointCloud cloud_; - boost::mutex mutex_; - boost::condition cloud_condition_; -}; + sensor_msgs::msg::PointCloud cloud_; + std::mutex mutex_; -void spinThread() -{ - ros::spin(); -} + std::condition_variable cloud_condition_; +}; // Check to make sure we can get a point cloud with at least 1 point in it -TEST_F(TestAssembler, non_zero_cloud_test) -{ +TEST_F(TestAssembler, non_zero_cloud_test) { // Wait until we get laser scans - boost::mutex::scoped_lock lock(mutex_); + std::unique_lock lock(mutex_); - while(n_.ok() && !got_cloud_) - { - cloud_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000)); + while (rclcpp::ok() && !got_cloud_) { + cloud_condition_.wait(lock); } - EXPECT_LT((unsigned int) 0, cloud_.points.size()); + EXPECT_LT((unsigned int)0, cloud_.points.size()); SUCCEED(); } -int main(int argc, char** argv) +int main(int argc, char ** argv) { printf("******* Starting application *********\n"); - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_assembler_node"); - - boost::thread spin_thread(spinThread); - + rclcpp::init(0, nullptr); int result = RUN_ALL_TESTS(); - - ros::shutdown(); - - spin_thread.join(); + rclcpp::shutdown(); return result; } diff --git a/test/test_laser_assembler.launch b/test/test_laser_assembler.launch deleted file mode 100644 index a46166c..0000000 --- a/test/test_laser_assembler.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - -