From 279240a4ce878efb8bdb4621d182f930e82c57aa Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 17:44:07 +0530 Subject: [PATCH 01/29] Modified CMakeLists.txt and package.xml according to ROS2 migration guide. --- CMakeLists.txt | 208 +++++++++++++++++++++++++++---------------------- package.xml | 43 +++++----- 2 files changed, 141 insertions(+), 110 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ae36340..fae0005 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,102 +1,128 @@ -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) +set( filters_FOUND 1 ) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators 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(laser_assembler_srv_gen REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(filters REQUIRED) + +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} + ${laser_assembler_srv_gen_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} + ${laser_assembler_srv_gen_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_srv src/laser_scan_assembler_srv.cpp) +# target_link_libraries(laser_scan_assembler_srv ${LIBS} ${Boost_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 - -if(CATKIN_ENABLE_TESTING) - find_package(rostest) +target_link_libraries(laser_scan_assembler ${LIBS}) +install( + TARGETS laser_scan_assembler + DESTINATION lib/${PROJECT_NAME}) + +#add_executable(merge_clouds src/merge_clouds.cpp) +#target_link_libraries(merge_clouds ${LIBS}) + +# add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp) +# target_link_libraries(point_cloud_assembler_srv ${LIBS}) + +#add_executable(point_cloud_assembler src/point_cloud_assembler.cpp) +#target_link_libraries(point_cloud_assembler ${LIBS}) +#install( +# TARGETS point_cloud_assembler +# DESTINATION lib/${PROJECT_NAME}) + +# add_executable(point_cloud2_assembler src/point_cloud2_assembler.cpp) +# target_link_libraries(point_cloud2_assembler ${LIBS}) +# install( + #TARGETS point_cloud2_assembler + #DESTINATION lib/${PROJECT_NAME}) + +include_directories(include) + +install(DIRECTORY "include/" + DESTINATION include) + +install(TARGETS laser_scan_assembler + #point_cloud_assembler + #point_cloud2_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(laser_geometry) +ament_export_dependencies(tf2_ros) +ament_export_dependencies(tf2) +ament_export_dependencies(message_filters) +ament_export_include_directories(${INCLUDE_DIRS}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) - 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) - - 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) + target_link_libraries(dummy_scan_producer ${LIBS}) + install( + TARGETS dummy_scan_producer + DESTINATION lib/${PROJECT_NAME}) + + ament_add_gtest(test_assembler test/test_assembler.cpp) + target_link_libraries(test_assembler ${LIBS}) + install( + TARGETS test_assembler + DESTINATION lib/${PROJECT_NAME}) + + add_executable(periodic_snapshotter examples/periodic_snapshotter.cpp) + target_link_libraries(periodic_snapshotter ${LIBS}) + 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/package.xml b/package.xml index ba0b295..76c901d 100644 --- a/package.xml +++ b/package.xml @@ -1,39 +1,44 @@ - + + 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 + 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 + laser_assembler_srv_gen + builtin_interfaces + + sensor_msgs + message_filters + rclcpp + tf2 + tf2_ros + message_filters + filters + laser_geometry + builtin_interfaces + laser_assembler_srv_gen + ament_cmake From 067deb903f7f6c9e7cd2895444012a680902bab5 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 17:46:51 +0530 Subject: [PATCH 02/29] ROS2 changes for launch file. --- launch/test_laser_assembler.launch.py | 68 +++++++++++++++++++++++++++ launch/test_laser_assembler.yaml | 7 +++ test/test_laser_assembler.launch | 15 ------ 3 files changed, 75 insertions(+), 15 deletions(-) create mode 100644 launch/test_laser_assembler.launch.py create mode 100644 launch/test_laser_assembler.yaml delete mode 100644 test/test_laser_assembler.launch diff --git a/launch/test_laser_assembler.launch.py b/launch/test_laser_assembler.launch.py new file mode 100644 index 0000000..6565329 --- /dev/null +++ b/launch/test_laser_assembler.launch.py @@ -0,0 +1,68 @@ + + +from launch import LaunchDescription +import launch_ros.actions +import os +import yaml +from launch.substitutions import EnvironmentVariable +import pathlib +import launch.actions +from launch.actions import DeclareLaunchArgument + + +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/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 @@ - - - - - - - - - - - - - - - From 7e9d35043a1186317f3c75176fc6c8c564c85150 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 18:00:16 +0530 Subject: [PATCH 03/29] Modified AssembleScans.srv and AssembleScans2.srv for ROS2. --- srv/AssembleScans.srv | 6 +++--- srv/AssembleScans2.srv | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/srv/AssembleScans.srv b/srv/AssembleScans.srv index b73949e..083e742 100644 --- a/srv/AssembleScans.srv +++ b/srv/AssembleScans.srv @@ -1,9 +1,9 @@ # The time interval on which we want to aggregate scans -time begin +uint64 begin # The end of the interval on which we want to assemble scans or clouds -time end +uint64 end --- # The point cloud holding the assembled clouds or scans. # This cloud is in the frame specified by the ~fixed_frame node parameter. # cloud is empty if no scans are received in the requested interval. -sensor_msgs/PointCloud cloud +sensor_msgs/msg/PointCloud cloud diff --git a/srv/AssembleScans2.srv b/srv/AssembleScans2.srv index 8071506..fcfd11f 100644 --- a/srv/AssembleScans2.srv +++ b/srv/AssembleScans2.srv @@ -1,9 +1,9 @@ # The time interval on which we want to aggregate scans -time begin +uint64 begin # The end of the interval on which we want to assemble scans or clouds -time end +uint64 end --- # The point cloud holding the assembled clouds or scans. # This cloud is in the frame specified by the ~fixed_frame node parameter. # cloud is empty if no scans are received in the requested interval. -sensor_msgs/PointCloud2 cloud +sensor_msgs/msg/PointCloud2 cloud From 654d367560f4710b9f368deb5a6709024c4b50d4 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 18:49:09 +0530 Subject: [PATCH 04/29] Added point_cloud_conversion.h and point_field_conversion.h files. These files were not there in sensor_msgs package of ros2 so just copied it from ROS1 and ported to ROS2 here. laser assembler uses convertPointCloudToPointCloud2() function from point_cloud_conversion.h file. --- .../laser_assembler/point_cloud_conversion.h | 169 ++++++++++++++++ .../laser_assembler/point_field_conversion.h | 180 ++++++++++++++++++ 2 files changed, 349 insertions(+) create mode 100644 include/laser_assembler/point_cloud_conversion.h create mode 100644 include/laser_assembler/point_field_conversion.h diff --git a/include/laser_assembler/point_cloud_conversion.h b/include/laser_assembler/point_cloud_conversion.h new file mode 100644 index 0000000..5bebc44 --- /dev/null +++ b/include/laser_assembler/point_cloud_conversion.h @@ -0,0 +1,169 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2010, 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 Willow Garage, Inc. 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. + * + * $Id$ + * + */ + +#ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H +#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H + +#include +#include +//#include +#include "point_field_conversion.h" + +/** + * \brief Convert between the old (sensor_msgs::PointCloud) and the new (sensor_msgs::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::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 ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)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 ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)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); +} +} +#endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H diff --git a/include/laser_assembler/point_field_conversion.h b/include/laser_assembler/point_field_conversion.h new file mode 100644 index 0000000..e9c2aad --- /dev/null +++ b/include/laser_assembler/point_field_conversion.h @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by the University of Osnabrück + * Copyright (c) 2015, University of Osnabrück + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. 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. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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. + * + * + * + * point_field_conversion.h + * + * Created on: 16.07.2015 + * Authors: Sebastian Pütz + */ + +#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H +#define SENSOR_MSGS_POINT_FIELD_CONVERSION_H + +/** + * \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; + } + } +} + +#endif /* point_field_conversion.h */ From fff899e0a2e3db526770d08f590ef3d02ba4c76f Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 18:53:42 +0530 Subject: [PATCH 05/29] Renamed base_assembler.h file to base_assembler.hpp. Renamed base_assembler_srv.h to base_assembler_srv.hpp. Also ported both files to ROS2 as per ROS2 migration guide. --- include/laser_assembler/base_assembler.hpp | 448 ++++++++++++++++++ .../laser_assembler/base_assembler_srv.hpp | 336 +++++++++++++ 2 files changed, 784 insertions(+) create mode 100644 include/laser_assembler/base_assembler.hpp create mode 100644 include/laser_assembler/base_assembler_srv.hpp diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp new file mode 100644 index 0000000..4ab4e71 --- /dev/null +++ b/include/laser_assembler/base_assembler.hpp @@ -0,0 +1,448 @@ +/********************************************************************* +* 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 "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_listener.h" +#include "tf2/exceptions.h" +#include +#include +#include "rclcpp/utilities.hpp" +//#include "tf2_ros/message_filter.h" // TODO message_filter.h file in ros2'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 "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 "point_cloud_conversion.h" +#include "message_filters/subscriber.h" + +#include + +// Service +#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" +#include "laser_assembler_srv_gen/srv/assemble_scans2.hpp" + +#include +#include + +#include "math.h" + +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf +#define ROS_FATAL printf + +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, 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: + tf2::BufferCore tfBuffer; + tf2_ros::TransformListener *tf_; + tf2_ros::MessageFilter* tf_filter_; + rclcpp::Node::SharedPtr private_ns_; + rclcpp::Node *n_; + +private: + // ROS Input/Ouptut Handling + rclcpp::Service::SharedPtr assemble_scans_server_; + rclcpp::Service::SharedPtr assemble_scans_server2_; + rclcpp::Service::SharedPtr build_cloud_server_; + //rclcpp::Service::SharedPtr build_cloud_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 std::shared_ptr& scan_ptr) ; + + //! \brief Service Callback function called whenever we need to build a cloud + + bool buildCloud(std::shared_ptr request, + std::shared_ptr response); + //bool assembleScans(laser_assembler_srv_gen::srv::AssembleScans::Request& req, laser_assembler_srv_gen::srv::AssembleScans::Response& resp); + bool assembleScans(std::shared_ptr request, + std::shared_ptr response); + bool assembleScans2(std::shared_ptr request, + std::shared_ptr response); + //bool buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp); + + + //! \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, rclcpp::Node::SharedPtr node_) +{ + // **** Initialize TransformListener **** + double tf_cache_time_secs; + private_ns_ = node_; + n_ = node_.get(); + + printf("BaseAssembler::BaseAssembler constructor "); + + private_ns_->get_parameter_or("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 tf2_ros::TransformListener(tfBuffer); + 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_->get_parameter_or(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_->get_parameter_or("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_->get_parameter_or("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 ***** + 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); + + auto buildCloudCallback = + [this]( std::shared_ptr request, + std::shared_ptr response) -> bool + { + this->buildCloud(request, response); + return true; + }; + + build_cloud_server_ = n_->create_service("build_cloud", buildCloudCallback); + + auto assembleScans2Callback = + [this]( std::shared_ptr request, + std::shared_ptr response) -> bool + { + this->assembleScans2(request, response); + return true; + }; + + assemble_scans_server2_ = n_->create_service("assemble_scans2", assembleScans2Callback); + + // ***** 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); + 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() +{ + 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"); + 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) +{ + ROS_DEBUG("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) + { + 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: %4lu Points: %10u\n", scan_hist_.size(), total_pts_) ; + + scan_hist_mutex_.unlock() ; + ROS_DEBUG("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; icloud.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; icloud.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; kcloud.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() ; + + ROS_DEBUG("\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 ; +} + +template +bool BaseAssembler::buildCloud(std::shared_ptr req, std::shared_ptr resp) +{ + ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); + return assembleScans(req, resp); +} + +template +bool BaseAssembler::assembleScans2(std::shared_ptr req, + std::shared_ptr resp) +{ + std::shared_ptr tmp_req; + std::shared_ptr 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; +} + +/* +template +bool BaseAssembler::buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) +{ + ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); + return assembleScans2(req, resp); +} + +template +bool BaseAssembler::assembleScans2(laser_assembler_srv_gen::srv::AssembleScans2::Request& req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) +{ + laser_assembler_srv_gen::srv::AssembleScans::Request tmp_req; + laser_assembler_srv_gen::srv::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_srv.hpp b/include/laser_assembler/base_assembler_srv.hpp new file mode 100644 index 0000000..570d8d1 --- /dev/null +++ b/include/laser_assembler/base_assembler_srv.hpp @@ -0,0 +1,336 @@ +/********************************************************************* +* 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 "rclcpp/rclcpp.hpp" +#include "tf2_ros/transform_listener.h" +//#include "tf2_ros/message_filter.h" +#include "laser_assembler/message_filter.hpp" // TODO +#include "sensor_msgs/msg/point_cloud.hpp" +#include "message_filters/subscriber.h" + +#include + +// Service +#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" + +#include "boost/thread.hpp" +#include "math.h" + +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + +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 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(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) + 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::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) + { + 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(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%u. BufferSize: %lu. Points in cloud: %lu", start_index, past_end_index, scan_hist_.size(), resp.cloud.points.size()) ; + return true ; +} + +} From 8b43aa297622caed2156e2ba51f8293097511fd2 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 18:56:22 +0530 Subject: [PATCH 06/29] Renamed base_assembler.h file to base_assembler.hpp and base_assembler_srv.h file to base_assembler_srv.hpp in previous commit so removed it here. --- include/laser_assembler/base_assembler.h | 373 ------------------- include/laser_assembler/base_assembler_srv.h | 330 ---------------- 2 files changed, 703 deletions(-) delete mode 100644 include/laser_assembler/base_assembler.h delete mode 100644 include/laser_assembler/base_assembler_srv.h 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_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 ; -} - -} From 47f192d06fe3936a195e209dcbb3e0330cfb8a6a Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 18:59:14 +0530 Subject: [PATCH 07/29] Added message_filter.hpp file to include directory of this package because message_filter.h file from tf2_ros package of bouncy release was not ported to ROS2. so picked it from link https://github.com/ros2/geometry2/blob/ros2/tf2_ros/include/tf2_ros/message_filter.h and fixed some erros related to time conversion. --- include/laser_assembler/message_filter.hpp | 679 +++++++++++++++++++++ 1 file changed, 679 insertions(+) create mode 100644 include/laser_assembler/message_filter.hpp diff --git a/include/laser_assembler/message_filter.hpp b/include/laser_assembler/message_filter.hpp new file mode 100644 index 0000000..fd2d266 --- /dev/null +++ b/include/laser_assembler/message_filter.hpp @@ -0,0 +1,679 @@ +/* + * Copyright (c) 2010, 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, Inc. 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 Josh Faust */ + +#ifndef TF2_ROS_MESSAGE_FILTER_H +#define TF2_ROS_MESSAGE_FILTER_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#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, +}; +} + +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(); + //rclcpp::Time stamp = mt::TimeStamp::value(*message); + 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; + tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, + target_frame, frame_id, tf2::timeFromSec( + stamp.nanoseconds()/1e+9)); // TODO + 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)); // TODO + 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)); // TODO + + 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))) { // TODO + 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 +#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 +#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); // TODO + } + + 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 + +#endif From d1ae9deb83f1afbcc06e31786b0b66c0e73f23f0 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 19:11:45 +0530 Subject: [PATCH 08/29] Added service code which generates structure for request and response part of the communication. Request structure which is the type of the request part of the service Response structure which is the type of the request part of the service --- laser_assembler_srv_gen/CHANGELOG.rst | 0 laser_assembler_srv_gen/CMakeLists.txt | 22 +++++++++++ laser_assembler_srv_gen/mainpage.dox | 0 laser_assembler_srv_gen/package.xml | 39 +++++++++++++++++++ laser_assembler_srv_gen/srv/AssembleScans.srv | 9 +++++ .../srv/AssembleScans2.srv | 9 +++++ 6 files changed, 79 insertions(+) create mode 100644 laser_assembler_srv_gen/CHANGELOG.rst create mode 100644 laser_assembler_srv_gen/CMakeLists.txt create mode 100644 laser_assembler_srv_gen/mainpage.dox create mode 100644 laser_assembler_srv_gen/package.xml create mode 100644 laser_assembler_srv_gen/srv/AssembleScans.srv create mode 100644 laser_assembler_srv_gen/srv/AssembleScans2.srv diff --git a/laser_assembler_srv_gen/CHANGELOG.rst b/laser_assembler_srv_gen/CHANGELOG.rst new file mode 100644 index 0000000..e69de29 diff --git a/laser_assembler_srv_gen/CMakeLists.txt b/laser_assembler_srv_gen/CMakeLists.txt new file mode 100644 index 0000000..151167f --- /dev/null +++ b/laser_assembler_srv_gen/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.5) +project(laser_assembler_srv_gen) + +# Support C++14 +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") + +if(NOT WIN32) + set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra") +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} "srv/AssembleScans.srv" + "srv/AssembleScans2.srv" + DEPENDENCIES builtin_interfaces sensor_msgs) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/laser_assembler_srv_gen/mainpage.dox b/laser_assembler_srv_gen/mainpage.dox new file mode 100644 index 0000000..e69de29 diff --git a/laser_assembler_srv_gen/package.xml b/laser_assembler_srv_gen/package.xml new file mode 100644 index 0000000..2d38eb8 --- /dev/null +++ b/laser_assembler_srv_gen/package.xml @@ -0,0 +1,39 @@ + + + + laser_assembler_srv_gen + 1.8.4 + + Generates Assemble scans service. + + Jonathan Binney + BSD + + http://wiki.ros.org/laser_assembler + + https://github.com/ros-perception/laser_assembler.git + Vijay Pradeep + + ament_cmake + sensor_msgs + builtin_interfaces + rosidl_default_generators + + rclcpp + bondcpp + rosidl_default_runtime + sensor_msgs + + builtin_interfaces + rclcpp + + + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/laser_assembler_srv_gen/srv/AssembleScans.srv b/laser_assembler_srv_gen/srv/AssembleScans.srv new file mode 100644 index 0000000..3c7b31b --- /dev/null +++ b/laser_assembler_srv_gen/srv/AssembleScans.srv @@ -0,0 +1,9 @@ +# The time interval on which we want to aggregate scans +int64 begin +# The end of the interval on which we want to assemble scans or clouds +int64 end +--- +# The point cloud holding the assembled clouds or scans. +# This cloud is in the frame specified by the ~fixed_frame node parameter. +# cloud is empty if no scans are received in the requested interval. +sensor_msgs/PointCloud cloud diff --git a/laser_assembler_srv_gen/srv/AssembleScans2.srv b/laser_assembler_srv_gen/srv/AssembleScans2.srv new file mode 100644 index 0000000..e2bafd1 --- /dev/null +++ b/laser_assembler_srv_gen/srv/AssembleScans2.srv @@ -0,0 +1,9 @@ +# The time interval on which we want to aggregate scans +int64 begin +# The end of the interval on which we want to assemble scans or clouds +int64 end +--- +# The point cloud holding the assembled clouds or scans. +# This cloud is in the frame specified by the ~fixed_frame node parameter. +# cloud is empty if no scans are received in the requested interval. +sensor_msgs/PointCloud2 cloud From 3b9f7a7f89595210da8805e7209b9e6ef97e3423 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 19:25:32 +0530 Subject: [PATCH 09/29] Modified periodic_snapshotter.cpp file as per ros2 migration guide. Only porting is done but not tested. --- examples/periodic_snapshotter.cpp | 131 ++++++++++++++++++++---------- 1 file changed, 89 insertions(+), 42 deletions(-) diff --git a/examples/periodic_snapshotter.cpp b/examples/periodic_snapshotter.cpp index 0c6ef17..ad96e86 100644 --- a/examples/periodic_snapshotter.cpp +++ b/examples/periodic_snapshotter.cpp @@ -33,15 +33,25 @@ *********************************************************************/ #include -#include - +#include "rclcpp/rclcpp.hpp" +#include +#include // Services -#include "laser_assembler/AssembleScans.h" +#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" // Messages -#include "sensor_msgs/PointCloud.h" - - +#include "sensor_msgs/msg/point_cloud.hpp" +#include "rcutils/cmdline_parser.h" +#include "rclcpp/clock.hpp" +#include "std_msgs/msg/string.hpp" + +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + +using namespace std::chrono_literals; +using namespace std::placeholders; /*** * This a simple test app that requests a point cloud from the * point_cloud_assembler every 4 seconds, and then publishes the @@ -50,59 +60,95 @@ 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); - - // Create the service client for calling the assembler - client_ = n_.serviceClient("assemble_scans"); + std::cerr<< " Inside PeriodicSnapshotter constructor " << "\n"; - // Start the timer that will trigger the processing loop (timerCallback) - timer_ = n_.createTimer(ros::Duration(5,0), &PeriodicSnapshotter::timerCallback, this); + client_ = this->create_client("build_cloud"); + + if (!client_->wait_for_service(20s)) { + printf("Service not available after waiting"); + return; + } + printf(" Service assemble_scans started successfully"); + //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + + // Create a function for when messages are to be sent. + auto timer_callback = [this]() -> void { + // msg_->name = "Hello World: " + std::to_string(count_++); + // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg_->status) + //RCLCPP_INFO(this->get_logger(), "Publishing: namespce %s and name is %s ", + // this->get_namespace(), this->get_name()); + + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + this->timerCallback(); + }; + + // 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_) { + 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(); + + request->begin = last_time.sec; + last_time = rclcpp::Clock().now(); + request->end = last_time.sec; + + + using ServiceResponseFuture = + rclcpp::Client::SharedFuture; + + auto response_received_callback = [this](ServiceResponseFuture future) { + auto result = future.get(); + printf("\n Cloud points size = %ld\n", result.get()->cloud.points.size()); + //RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum) + if (result.get()->cloud.points.size() > 0 ) { + + result.get()->cloud; + pub_->publish(result.get()->cloud); + } + else + ROS_INFO("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_; } ; @@ -112,12 +158,13 @@ using namespace laser_assembler ; 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; } From 81c094bc1c1170a2de0129beda17ae60cf94d4c9 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 19:27:32 +0530 Subject: [PATCH 10/29] Ported laser_scan_assembler.cpp file to ROS2. --- src/laser_scan_assembler.cpp | 80 ++++++++++++++++++++++++++---------- 1 file changed, 58 insertions(+), 22 deletions(-) diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index 6d76e78..192bdb0 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -33,13 +33,30 @@ *********************************************************************/ -#include "laser_geometry/laser_geometry.h" -#include "sensor_msgs/LaserScan.h" -#include "laser_assembler/base_assembler.h" +#include "laser_geometry/laser_geometry.hpp" + +//#include "laser_assembler/laser_geometry.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "laser_assembler/base_assembler.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include +#include +#include +#include +#include #include "filters/filter_chain.h" +#include "rclcpp/time.hpp" using namespace laser_geometry; using namespace std ; +using namespace std::chrono; + +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + +#define TIME rclcpp::Time namespace laser_assembler { @@ -47,13 +64,13 @@ namespace laser_assembler /** * \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") + LaserScanAssembler(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); + 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_); @@ -64,7 +81,7 @@ class LaserScanAssembler : public BaseAssembler 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)); } } @@ -73,22 +90,40 @@ class LaserScanAssembler : public BaseAssembler } - 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_); - // convert laser scan to point cloud + 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(); + + // TODO ignore_laser_skew_ this case is not handled right now, as there is no transformPointCloud support for PointCloud in ROS2. 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) { + + // TODO transform PointCloud + /* tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); + TIME start_time = cloud_out.header.stamp; + std::chrono::nanoseconds start(start_time.nanoseconds()); + std::chrono::time_point st(start); + geometry_msgs::msg::TransformStamped transform = tfBuffer.lookupTransform(fixed_frame_id, + cloud_out.header.frame_id, st); + sensor_msgs::msg::PointCloud2 cloudout; + tf2::doTransform(cloud_out, cloud_out, transform);*/ + } } else // Do it the slower (more accurate) way { @@ -96,19 +131,19 @@ class LaserScanAssembler : public BaseAssembler 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); + 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()); + rclcpp::Duration cur_tolerance = rclcpp::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()); + ROS_DEBUG("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,11 +156,11 @@ 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 + 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::LaserScan scan_filtered_; + filters::FilterChain filter_chain_; + mutable sensor_msgs::msg::LaserScan scan_filtered_/*scan_filtered_*/; }; @@ -135,9 +170,10 @@ using namespace laser_assembler ; int main(int argc, char **argv) { - ros::init(argc, argv, "laser_scan_assembler"); - LaserScanAssembler pc_assembler; - ros::spin(); + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("laser_scan_assembler"); + LaserScanAssembler pc_assembler(node); + rclcpp::spin(node); return 0; } From 1bdc082ae831343d58abc10fb82ee7b21869762f Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 19:28:56 +0530 Subject: [PATCH 11/29] Modified following files to ROS2 as per ROS2 migration guide. - src/laser_scan_assembler_srv.cpp - src/merge_clouds.cpp - src/point_cloud2_assembler.cpp - src/point_cloud_assembler.cpp - src/point_cloud_assembler_srv.cpp --- src/laser_scan_assembler_srv.cpp | 26 ++++++----- src/merge_clouds.cpp | 74 +++++++++++++++++-------------- src/point_cloud2_assembler.cpp | 27 ++++++----- src/point_cloud_assembler.cpp | 26 +++++++---- src/point_cloud_assembler_srv.cpp | 12 +++-- 5 files changed, 98 insertions(+), 67 deletions(-) diff --git a/src/laser_scan_assembler_srv.cpp b/src/laser_scan_assembler_srv.cpp index 5137e04..514df51 100644 --- a/src/laser_scan_assembler_srv.cpp +++ b/src/laser_scan_assembler_srv.cpp @@ -33,11 +33,17 @@ *********************************************************************/ -#include "laser_geometry/laser_geometry.h" -#include "sensor_msgs/LaserScan.h" -#include "laser_assembler/base_assembler_srv.h" +#include "laser_geometry/laser_geometry.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp"; +#include "laser_assembler/base_assembler_srv.hpp" #include "filters/filter_chain.h" +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + using namespace laser_geometry; using namespace std ; @@ -54,13 +60,13 @@ namespace laser_assembler * \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_); @@ -71,12 +77,12 @@ class LaserScanAssemblerSrv : public BaseAssemblerSrv } - 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_); @@ -100,8 +106,8 @@ class LaserScanAssemblerSrv : public BaseAssemblerSrv 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_; }; diff --git a/src/merge_clouds.cpp b/src/merge_clouds.cpp index bb84278..5a142ff 100644 --- a/src/merge_clouds.cpp +++ b/src/merge_clouds.cpp @@ -40,18 +40,24 @@ potentially from different sensors **/ +// #include +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "sensor_msgs/msg/point_cloud.hpp" +//#include +#include "laser_assembler/message_filter.hpp" // TODO message_filter.h file in ros2's tf2_ros package is not ported to ros2 yet. +#include -#include -#include -#include -#include - -#include -#include -#include +#include +#include #include +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + class MergeClouds { public: @@ -60,7 +66,7 @@ class MergeClouds 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; @@ -74,17 +80,17 @@ class MergeClouds if (max_freq_ > 0.0) { - timer_ = nh_.createTimer(ros::Duration(1.0/max_freq_), boost::bind(&MergeClouds::onTimer, this, _1)); + timer_ = nh_.createTimer(rclcpp::Duration(1.0/max_freq_), std::bind(&MergeClouds::onTimer, this, _1)); haveTimer_ = true; } 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) @@ -108,7 +114,7 @@ class MergeClouds newCloud1_ = false; newCloud2_ = false; - sensor_msgs::PointCloud out; + sensor_msgs::msg::PointCloud out; if (cloud1_.header.stamp > cloud2_.header.stamp) out.header = cloud1_.header; else @@ -125,8 +131,8 @@ class MergeClouds 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()); + //ROS_ASSERT(cloud1_.channels[i].values.size() == cloud1_.points.size()); + //ROS_ASSERT(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; @@ -142,7 +148,7 @@ 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_); @@ -152,7 +158,7 @@ class MergeClouds publishClouds(); } - void receiveCloud2(const sensor_msgs::PointCloudConstPtr &cloud) + void receiveCloud2(const sensor_msgs::msg::PointCloudConstPtr &cloud) { lock2_.lock(); processCloud(cloud, cloud2_); @@ -162,7 +168,7 @@ class MergeClouds 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) tf_.transformPointCloud(output_frame_, *cloud, cloudOut); @@ -180,29 +186,29 @@ class MergeClouds double max_freq_; std::string output_frame_; - message_filters::Subscriber sub1_; - message_filters::Subscriber sub2_; - boost::shared_ptr > tf_filter1_; - boost::shared_ptr > tf_filter2_; + message_filters::Subscriber sub1_; + message_filters::Subscriber sub2_; + std::shared_ptr > tf_filter1_; + std::shared_ptr > tf_filter2_; bool newCloud1_; bool newCloud2_; - sensor_msgs::PointCloud cloud1_; - sensor_msgs::PointCloud cloud2_; - boost::mutex lock1_; - boost::mutex lock2_; + sensor_msgs::msg::PointCloud cloud1_; + sensor_msgs::msg::PointCloud cloud2_; + std::mutex lock1_; + std::mutex lock2_; }; 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..850a839 100644 --- a/src/point_cloud2_assembler.cpp +++ b/src/point_cloud2_assembler.cpp @@ -33,11 +33,17 @@ *********************************************************************/ -#include "laser_assembler/base_assembler.h" -#include +#include "laser_assembler/base_assembler.hpp" +//#include // TODO +#include "laser_assembler/point_cloud_conversion.h" using namespace std ; +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + namespace laser_assembler { @@ -47,10 +53,10 @@ namespace laser_assembler * params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloud2Assembler : public BaseAssembler +class PointCloud2Assembler : public BaseAssembler { public: - PointCloud2Assembler() : BaseAssembler("max_clouds") + PointCloud2Assembler(rclcpp::Node::SharedPtr node) : BaseAssembler("max_clouds", node) { } @@ -60,14 +66,14 @@ class PointCloud2Assembler : public BaseAssembler } - unsigned int GetPointsInScan(const sensor_msgs::PointCloud2& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::PointCloud2& scan) { 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 ; @@ -83,10 +89,11 @@ using namespace laser_assembler ; 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"); + 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..a61881b 100644 --- a/src/point_cloud_assembler.cpp +++ b/src/point_cloud_assembler.cpp @@ -33,10 +33,15 @@ *********************************************************************/ -#include "laser_assembler/base_assembler.h" +#include "laser_assembler/base_assembler.hpp" -using namespace std ; +using namespace std; + +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf namespace laser_assembler { @@ -47,10 +52,10 @@ namespace laser_assembler * params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloudAssembler : public BaseAssembler +class PointCloudAssembler : public BaseAssembler { public: - PointCloudAssembler() : BaseAssembler("max_clouds") + PointCloudAssembler(rclcpp::Node::SharedPtr node) : BaseAssembler("max_clouds", node) { } @@ -60,14 +65,16 @@ class PointCloudAssembler : public BaseAssembler } - unsigned int GetPointsInScan(const sensor_msgs::PointCloud& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::PointCloud& scan) { 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 std::string& fixed_frame_id, const sensor_msgs::msg::PointCloud& scan_in, sensor_msgs::msg::PointCloud& cloud_out) { + // TODO tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out) ; + //tf2::doTransform(scan_in, cloud_out, tfBuffer.lookupTransform(fixed_frame_id, tf2::getFrameId(scan_in), tf2::getTimestamp(scan_in))); return ; } @@ -81,10 +88,11 @@ using namespace laser_assembler ; 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"); + 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..4e94a40 100644 --- a/src/point_cloud_assembler_srv.cpp +++ b/src/point_cloud_assembler_srv.cpp @@ -33,11 +33,15 @@ *********************************************************************/ -#include "laser_assembler/base_assembler_srv.h" - +#include "laser_assembler/base_assembler_srv.hpp" using namespace std; +#define ROS_ERROR printf +#define ROS_INFO printf +#define ROS_WARN printf +#define ROS_DEBUG printf + namespace laser_assembler { @@ -47,7 +51,7 @@ namespace laser_assembler * params * * (Several params are inherited from BaseAssemblerSrv) */ -class PointCloudAssemblerSrv : public BaseAssemblerSrv +class PointCloudAssemblerSrv : public BaseAssemblerSrv { public: PointCloudAssemblerSrv() @@ -60,7 +64,7 @@ class PointCloudAssemblerSrv : public BaseAssemblerSrv } - unsigned int GetPointsInScan(const sensor_msgs::PointCloud& scan) + unsigned int GetPointsInScan(const sensor_msgs::msg::PointCloud& scan) { return scan.points.size() ; } From fdf95913bcc10deeff332cb8677f4a80441d16a8 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Mon, 17 Dec 2018 19:31:14 +0530 Subject: [PATCH 12/29] Ported test/dummy_scan_producer.cpp and test/test_assembler.cpp files to ROS2 and also tested it. non_zero_cloud_test passed successfully. --- test/dummy_scan_producer.cpp | 57 ++++++++------ test/test_assembler.cpp | 139 ++++++++++++++++++++++------------- 2 files changed, 124 insertions(+), 72 deletions(-) diff --git a/test/dummy_scan_producer.cpp b/test/dummy_scan_producer.cpp index 5c1e4c2..60c5fc3 100644 --- a/test/dummy_scan_producer.cpp +++ b/test/dummy_scan_producer.cpp @@ -38,25 +38,30 @@ * 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 +#include +#include +#include "tf2/LinearMath/Transform.h" -void runLoop() -{ - ros::NodeHandle nh; - ros::Publisher scan_pub = nh.advertise("dummy_scan", 100); - ros::Rate loop_rate(5); +#define ROS_INFO printf + +void runLoop(rclcpp::Node::SharedPtr node_) +{ + rclcpp::Rate loopRate(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; @@ -76,22 +81,30 @@ void runLoop() } // Keep sending scans until the assembler is done - while (nh.ok()) + while(rclcpp::ok()) { - scan.header.stamp = ros::Time::now(); - scan_pub.publish(scan); - broadcaster.sendTransform(tf::StampedTransform(laser_transform, scan.header.stamp, "dummy_laser_link", "dummy_base_link")); - loop_rate.sleep(); + scan.header.stamp = rclcpp::Clock().now(); + ROS_INFO("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(); ROS_INFO("Publishing scan"); } } int main(int argc, char** argv) { - ros::init(argc, argv, "scan_producer"); - ros::NodeHandle nh; - boost::thread run_thread(&runLoop); - ros::spin(); + printf("dummy scan producer\n"); + 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..5ac4e83 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -36,14 +36,14 @@ #include #include -#include -#include "sensor_msgs/PointCloud.h" -#include "sensor_msgs/LaserScan.h" -#include "laser_assembler/AssembleScans.h" -#include -#include - -using namespace ros; +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/point_cloud2.h" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" +#include + +#define ROS_INFO printf + using namespace sensor_msgs; using namespace std; @@ -53,21 +53,44 @@ class TestAssembler : public testing::Test { public: - NodeHandle n_; + rclcpp::Node::SharedPtr node; + //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + rclcpp::Client::SharedPtr client_; + + TestAssembler() {} + + 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()); + //ROS_INFO("Waiting for service [%s]", 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"); + if (!client_->wait_for_service(20s)) { + printf("Service not available after waiting"); + return; + } + printf(" Service assemble_scans started successfully"); + //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); 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_); + std::unique_lock lock(mutex_); if (!received_something_) { // Store the time of this first scan. Will be needed when we make the service call @@ -77,47 +100,69 @@ class TestAssembler : public testing::Test 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"); + + std::this_thread::sleep_for(0.2s); + auto request = std::make_shared(); + 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::SharedFuture; + + auto response_received_callback = [this](ServiceResponseFuture future) { + auto result = future.get(); + printf("\nCloud points size = %ld\n", result.get()->cloud.points.size()); + //RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum) + if (result.get()->cloud.points.size() > 0 ) { + + cloud_ = result.get()->cloud; + got_cloud_ = true; + cloud_condition_.notify_all(); + } + else + ROS_INFO("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_; + + std::condition_variable cloud_condition_; }; - -void spinThread() -{ - ros::spin(); -} - // Check to make sure we can get a point cloud with at least 1 point in it 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_) + while(rclcpp::ok() && !got_cloud_) { - cloud_condition_.timed_wait(lock, boost::posix_time::milliseconds(1000)); + cloud_condition_.wait(lock); } EXPECT_LT((unsigned int) 0, cloud_.points.size()); @@ -130,14 +175,8 @@ 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; } From 2b0d65fcbacffab81c7f39cfb9d8061e9ec897d3 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Tue, 18 Dec 2018 15:50:46 +0530 Subject: [PATCH 13/29] Modified message_filter.hpp and tested with crystal release. --- include/laser_assembler/message_filter.hpp | 26 +++++++++++----------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/include/laser_assembler/message_filter.hpp b/include/laser_assembler/message_filter.hpp index fd2d266..5b0d02e 100644 --- a/include/laser_assembler/message_filter.hpp +++ b/include/laser_assembler/message_filter.hpp @@ -265,7 +265,6 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi namespace mt = message_filters::message_traits; const MConstPtr & message = evt.getMessage(); - //rclcpp::Time stamp = mt::TimeStamp::value(*message); std::string frame_id = stripSlash(mt::FrameId::value(*message)); rclcpp::Time stamp = mt::TimeStamp::value(*message); @@ -291,7 +290,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi const std::string & target_frame = *it; tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, tf2::timeFromSec( - stamp.nanoseconds()/1e+9)); // TODO + stamp.seconds())); if (handle == 0xffffffffffffffffULL) { // never transformable messageDropped(evt, filter_failure_reasons::OutTheBack); @@ -304,7 +303,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi if (time_tolerance_.nanoseconds()) { handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, tf2::timeFromSec( - (stamp + time_tolerance_).nanoseconds()/1e+9)); // TODO + (stamp + time_tolerance_).seconds())); if (handle == 0xffffffffffffffffULL) { // never transformable messageDropped(evt, filter_failure_reasons::OutTheBack); @@ -335,7 +334,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi "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)); // TODO + mt::TimeStamp::value(*front.event.getMessage()).seconds()); V_TransformableRequestHandle::const_iterator it = front.handles.begin(); V_TransformableRequestHandle::const_iterator end = front.handles.end(); @@ -356,7 +355,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } 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_); + frame_id.c_str(), stamp.seconds(), message_count_); ++incoming_message_count_; } @@ -375,7 +374,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi //Time t = rclcpp::Clock::now(); rclcpp::Clock system_clock(RCL_SYSTEM_TIME); rclcpp::Time t = system_clock.now(); - add(MEvent(message, header, t)); + //add(MEvent(message, header, t)); + add(MEvent(message, t)); } /** @@ -464,14 +464,14 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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))) { // TODO + if (!bc_.canTransform(target, frame_id, tf2::timeFromSec(stamp.seconds()))) { can_transform = false; break; } if (time_tolerance_.nanoseconds()) { if (!bc_.canTransform(target, frame_id, - tf2::timeFromSec((stamp + time_tolerance_).nanoseconds()/1e+9))) + tf2::timeFromSec((stamp + time_tolerance_).seconds()))) { can_transform = false; break; @@ -486,7 +486,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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); + frame_id.c_str(), stamp.seconds(), message_count_ - 1); ++successful_transform_count_; messageReady(info.event); @@ -494,7 +494,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi ++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); + frame_id.c_str(), stamp.seconds(), message_count_ - 1); messageDropped(info.event, filter_failure_reasons::Unknown); } @@ -532,7 +532,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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()); + last_out_the_back_stamp_.seconds(), last_out_the_back_frame_.c_str()); } } } @@ -608,8 +608,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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); // TODO + RCLCPP_INFO(node_->get_logger(), "[%s] Drop message: frame '%s' at time %.3f for reason(%d)", + __func__, frame_id.c_str(), stamp.seconds(), reason); } static std::string stripSlash(const std::string & in) From 2b09d96a6599509eef6cd3c5e4c4cd1170b97dc6 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Thu, 20 Dec 2018 18:56:22 +0530 Subject: [PATCH 14/29] cpp lint and python flake8 changes for laser_assembler. --- CMakeLists.txt | 10 +- examples/periodic_snapshotter.cpp | 138 ++--- include/laser_assembler/base_assembler.hpp | 572 ++++++++++-------- .../laser_assembler/base_assembler_srv.hpp | 391 ++++++------ include/laser_assembler/message_filter.hpp | 326 +++++----- .../laser_assembler/point_cloud_conversion.h | 169 ------ .../point_cloud_conversion.hpp | 174 ++++++ .../laser_assembler/point_field_conversion.h | 180 ------ .../point_field_conversion.hpp | 244 ++++++++ launch/test_laser_assembler.launch.py | 79 ++- package.xml | 2 + src/laser_scan_assembler.cpp | 174 +++--- src/laser_scan_assembler_srv.cpp | 128 ++-- src/merge_clouds.cpp | 193 +++--- src/point_cloud2_assembler.cpp | 97 ++- src/point_cloud_assembler.cpp | 96 ++- src/point_cloud_assembler_srv.cpp | 82 +-- test/dummy_scan_producer.cpp | 77 +-- test/test_assembler.cpp | 188 +++--- 19 files changed, 1647 insertions(+), 1673 deletions(-) delete mode 100644 include/laser_assembler/point_cloud_conversion.h create mode 100644 include/laser_assembler/point_cloud_conversion.hpp delete mode 100644 include/laser_assembler/point_field_conversion.h create mode 100644 include/laser_assembler/point_field_conversion.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fae0005..5f24b90 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(tf2 REQUIRED) find_package(laser_assembler_srv_gen REQUIRED) find_package(geometry_msgs REQUIRED) find_package(filters REQUIRED) - + set(INCLUDE_DIRS include ${ament_cmake_INCLUDE_DIRS} ${rosidl_default_generators_INCLUDE_DIRS} ${signals_INCLUDE_DIRS} ${builtin_interfaces_INCLUDE_DIRS} ${rclcpp_INCLUDE_DIRS} @@ -36,12 +36,12 @@ set(INCLUDE_DIRS include ${ament_cmake_INCLUDE_DIRS} include_directories(${INCLUDE_DIRS}) set(LIBS ${rclcpp_LIBRARIES} ${ament_cmake_LIBRARIES} - ${rosidl_default_generators_LIBRARIES} ${signals_LIBRARIES} + ${rosidl_default_generators_LIBRARIES} ${signals_LIBRARIES} ${laser_assembler_srv_gen_LIBRARIES} ${std_msgs_LIBRARIES} - ${system_LIBRARIES} ${tf2_ros_LIBRARIES} ${tf2_LIBRARIES} + ${system_LIBRARIES} ${tf2_ros_LIBRARIES} ${tf2_LIBRARIES} ${laser_geometry_LIBRARIES} ${geometry_msgs_LIBRARIES} - ${message_filters_LIBRARIES} ${sensor_msgs_LIBRARIES} + ${message_filters_LIBRARIES} ${sensor_msgs_LIBRARIES} ${filters_LIBRARIES}) # add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp) @@ -116,7 +116,7 @@ if(BUILD_TESTING) install( TARGETS test_assembler DESTINATION lib/${PROJECT_NAME}) - + add_executable(periodic_snapshotter examples/periodic_snapshotter.cpp) target_link_libraries(periodic_snapshotter ${LIBS}) install( diff --git a/examples/periodic_snapshotter.cpp b/examples/periodic_snapshotter.cpp index ad96e86..661a2f7 100644 --- a/examples/periodic_snapshotter.cpp +++ b/examples/periodic_snapshotter.cpp @@ -1,48 +1,30 @@ -/********************************************************************* -* 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 "rclcpp/rclcpp.hpp" #include -#include +#include // Services #include "laser_assembler_srv_gen/srv/assemble_scans.hpp" +#include "rclcpp/rclcpp.hpp" // Messages -#include "sensor_msgs/msg/point_cloud.hpp" -#include "rcutils/cmdline_parser.h" #include "rclcpp/clock.hpp" +#include "rcutils/cmdline_parser.h" +#include "sensor_msgs/msg/point_cloud.hpp" #include "std_msgs/msg/string.hpp" #define ROS_ERROR printf @@ -50,8 +32,6 @@ #define ROS_WARN printf #define ROS_DEBUG printf -using namespace std::chrono_literals; -using namespace std::placeholders; /*** * This a simple test app that requests a point cloud from the * point_cloud_assembler every 4 seconds, and then publishes the @@ -60,37 +40,39 @@ using namespace std::placeholders; namespace laser_assembler { -class PeriodicSnapshotter: public rclcpp::Node +class PeriodicSnapshotter : public rclcpp::Node { - public: - explicit PeriodicSnapshotter(const std::string & service_name) : Node(service_name) { - std::cerr<< " Inside PeriodicSnapshotter constructor " << "\n"; + std::cerr << " Inside PeriodicSnapshotter constructor " << + "\n"; + + client_ = this->create_client( + "build_cloud"); - client_ = this->create_client("build_cloud"); - + using namespace std::chrono_literals; if (!client_->wait_for_service(20s)) { printf("Service not available after waiting"); return; } printf(" Service assemble_scans started successfully"); - //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); - + // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + // Create a function for when messages are to be sent. auto timer_callback = [this]() -> void { // msg_->name = "Hello World: " + std::to_string(count_++); // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg_->status) - //RCLCPP_INFO(this->get_logger(), "Publishing: namespce %s and name is %s ", - // this->get_namespace(), this->get_name()); + // RCLCPP_INFO(this->get_logger(), "Publishing: namespce %s and name is %s + // ", + // this->get_namespace(), this->get_name()); // Put the message into a queue to be processed by the middleware. // This call is non-blocking. this->timerCallback(); }; - + // 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; @@ -98,65 +80,65 @@ class PeriodicSnapshotter: public rclcpp::Node "/assembled_cloud", custom_qos_profile); // Use a timer to schedule periodic message publishing. - timer_ = this->create_wall_timer(std::chrono::milliseconds(25), timer_callback); + 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 builtin_interfaces::msg::Time& 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 - auto request = std::make_shared(); - + auto request = std::make_shared< + laser_assembler_srv_gen::srv::AssembleScans::Request>(); + request->begin = last_time.sec; last_time = rclcpp::Clock().now(); request->end = last_time.sec; - - - using ServiceResponseFuture = - rclcpp::Client::SharedFuture; - - auto response_received_callback = [this](ServiceResponseFuture future) { + + using ServiceResponseFuture = rclcpp::Client< + laser_assembler_srv_gen::srv::AssembleScans>::SharedFuture; + + auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get(); - printf("\n Cloud points size = %ld\n", result.get()->cloud.points.size()); - //RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum) - if (result.get()->cloud.points.size() > 0 ) { - + printf( + "\n Cloud points size = %ld\n", + result.get()->cloud.points.size()); + // RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", + // future.get()->sum) + if (result.get()->cloud.points.size() > 0) { result.get()->cloud; pub_->publish(result.get()->cloud); - } - else + } else { ROS_INFO("Got an empty cloud. Going to try again on the next scan"); + } }; - + auto result = client_->async_send_request(request); -} + } private: rclcpp::Publisher::SharedPtr pub_; - rclcpp::Client::SharedPtr client_; + 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) { rclcpp::init(argc, argv); auto service_name = std::string("periodic_snapshotter"); @@ -164,7 +146,7 @@ int main(int argc, char **argv) if (nullptr != cli_option) { service_name = std::string(cli_option); } - auto node = std::make_shared(service_name); + auto node = std::make_shared(service_name); rclcpp::spin(node); return 0; } diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index 4ab4e71..14bcbd6 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -1,63 +1,51 @@ -/********************************************************************* -* 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 +#ifndef LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ +#define LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ + +#include +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" -#include "tf2_ros/transform_listener.h" -#include "tf2/exceptions.h" -#include -#include #include "rclcpp/utilities.hpp" -//#include "tf2_ros/message_filter.h" // TODO message_filter.h file in ros2'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 "message_filter.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 ros2'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 "point_cloud_conversion.h" +// #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 +#include "laser_assembler/point_cloud_conversion.hpp" // Service #include "laser_assembler_srv_gen/srv/assemble_scans.hpp" #include "laser_assembler_srv_gen/srv/assemble_scans2.hpp" -#include -#include - -#include "math.h" - #define ROS_ERROR printf #define ROS_INFO printf #define ROS_WARN printf @@ -68,14 +56,17 @@ namespace laser_assembler { /** - * \brief Maintains a history of point clouds and generates an aggregate point cloud upon request + * \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, rclcpp::Node::SharedPtr node_); - ~BaseAssembler() ; + BaseAssembler( + const std::string & max_size_param_name, + rclcpp::Node::SharedPtr node_); + ~BaseAssembler(); /** * \brief Tells the assembler to start listening to input data @@ -84,339 +75,407 @@ class BaseAssembler * 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(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 ; + 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 + * 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 ; + virtual void ConvertToCloud( + const std::string & fixed_frame_id, + const T & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) = 0; protected: tf2::BufferCore tfBuffer; - tf2_ros::TransformListener *tf_; - tf2_ros::MessageFilter* tf_filter_; + tf2_ros::TransformListener * tf_; + tf2_ros::MessageFilter * tf_filter_; rclcpp::Node::SharedPtr private_ns_; - rclcpp::Node *n_; + rclcpp::Node * n_; private: // ROS Input/Ouptut Handling - rclcpp::Service::SharedPtr assemble_scans_server_; - rclcpp::Service::SharedPtr assemble_scans_server2_; - rclcpp::Service::SharedPtr build_cloud_server_; - //rclcpp::Service::SharedPtr build_cloud_server2_; - + rclcpp::Service::SharedPtr + assemble_scans_server_; + rclcpp::Service::SharedPtr + assemble_scans_server2_; + rclcpp::Service::SharedPtr + build_cloud_server_; + // rclcpp::Service::SharedPtr + // build_cloud_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 std::shared_ptr& scan_ptr) ; + // void scansCallback(const tf::MessageNotifier::MessagePtr& scan_ptr, + // const T& testA) + virtual void msgCallback(const std::shared_ptr & scan_ptr); //! \brief Service Callback function called whenever we need to build a cloud - - bool buildCloud(std::shared_ptr request, - std::shared_ptr response); - //bool assembleScans(laser_assembler_srv_gen::srv::AssembleScans::Request& req, laser_assembler_srv_gen::srv::AssembleScans::Response& resp); - bool assembleScans(std::shared_ptr request, - std::shared_ptr response); - bool assembleScans2(std::shared_ptr request, - std::shared_ptr response); - //bool buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp); - + + bool buildCloud( + std::shared_ptr + request, + std::shared_ptr + response); + // bool assembleScans(laser_assembler_srv_gen::srv::AssembleScans::Request& + // req, laser_assembler_srv_gen::srv::AssembleScans::Response& resp); + bool assembleScans( + std::shared_ptr + request, + std::shared_ptr + response); + bool assembleScans2( + std::shared_ptr + request, + std::shared_ptr + response); + // bool buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& + // req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp); //! \brief Stores history of scans - std::deque scan_hist_ ; - std::mutex scan_hist_mutex_ ; + std::deque scan_hist_; + std::mutex scan_hist_mutex_; //! \brief The number points currently in the scan history - unsigned int total_pts_ ; + unsigned int total_pts_; //! \brief The max number of scans to store in the scan history - unsigned int max_scans_ ; + 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_ ; + 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, rclcpp::Node::SharedPtr node_) +template +BaseAssembler::BaseAssembler( + const std::string & max_size_param_name, + rclcpp::Node::SharedPtr node_) { // **** Initialize TransformListener **** double tf_cache_time_secs; private_ns_ = node_; n_ = node_.get(); - + printf("BaseAssembler::BaseAssembler constructor "); - + private_ns_->get_parameter_or("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) ; + + if (tf_cache_time_secs < 0) { + ROS_ERROR("Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs); + } tf_ = new tf2_ros::TransformListener(tfBuffer); - ROS_INFO("TF Cache Time: %f Seconds ", 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 ; + const int default_max_scans = 400; + int tmp_max_scans; - private_ns_->get_parameter_or(max_size_param_name, tmp_max_scans, default_max_scans); + private_ns_->get_parameter_or(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 ; + 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 + 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_->get_parameter_or("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")); + private_ns_->get_parameter_or("fixed_frame", fixed_frame_, + std::string("ERROR_NO_NAME")); - ROS_INFO("Fixed Frame: %s ", fixed_frame_.c_str()) ; + ROS_INFO("Fixed Frame: %s ", fixed_frame_.c_str()); - if (fixed_frame_ == "ERROR_NO_NAME") - ROS_ERROR("Need to set parameter fixed_frame") ; + if (fixed_frame_ == "ERROR_NO_NAME") { + ROS_ERROR("Need to set parameter fixed_frame"); + } // ***** Set downsample_factor ***** - int tmp_downsample_factor ; + int tmp_downsample_factor; private_ns_->get_parameter_or("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 ; + 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_); } - 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 ***** 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); - + [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); + auto buildCloudCallback = - [this]( std::shared_ptr request, - std::shared_ptr response) -> bool - { - this->buildCloud(request, response); - return true; - }; - - build_cloud_server_ = n_->create_service("build_cloud", buildCloudCallback); - + [this]( + std::shared_ptr + request, + std::shared_ptr + response) -> bool { + this->buildCloud(request, response); + return true; + }; + + build_cloud_server_ = + n_->create_service( + "build_cloud", buildCloudCallback); + auto assembleScans2Callback = - [this]( std::shared_ptr request, - std::shared_ptr response) -> bool - { - this->assembleScans2(request, response); - return true; - }; - - assemble_scans_server2_ = n_->create_service("assemble_scans2", assembleScans2Callback); + [this]( + std::shared_ptr + request, + std::shared_ptr< + laser_assembler_srv_gen::srv::AssembleScans2::Response> + response) -> bool { + this->assembleScans2(request, response); + return true; + }; + + assemble_scans_server2_ = + n_->create_service( + "assemble_scans2", assembleScans2Callback); // ***** Start Listening to Data ***** - // (Well, don't start listening just yet. Keep this as null until we actually start listening, when start() is called) + // (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) +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 - { + 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); - tf_filter_ = new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); - tf_filter_->registerCallback( std::bind(&BaseAssembler::msgCallback, this, std::placeholders::_1) ); + 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 +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 - { + 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"); - tf_filter_ = new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); - tf_filter_->registerCallback( std::bind(&BaseAssembler::msgCallback, this, std::placeholders::_1) ); + 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 +template BaseAssembler::~BaseAssembler() { - if (tf_filter_) + if (tf_filter_) { delete tf_filter_; + } - delete tf_ ; + delete tf_; } -template -void BaseAssembler::msgCallback(const std::shared_ptr& scan_ptr) +template +void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) { ROS_DEBUG("starting msgCallback"); - const T scan = *scan_ptr ; + const T scan = *scan_ptr; - sensor_msgs::msg::PointCloud cur_cloud ; + 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) - { - ROS_WARN("Transform Exception %s", ex.what()) ; - return ; + try { + ConvertToCloud(fixed_frame_, scan, + cur_cloud); // Convert scan into a point cloud + } catch (tf2::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 + 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 + 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_) ; + printf("Scans: %4lu Points: %10u\n", scan_hist_.size(), total_pts_); - scan_hist_mutex_.unlock() ; + scan_hist_mutex_.unlock(); ROS_DEBUG("done with msgCallback"); } -template -bool BaseAssembler::assembleScans(std::shared_ptr req, - std::shared_ptr resp) +template +bool BaseAssembler::assembleScans( + std::shared_ptr req, + std::shared_ptr + resp) { - scan_hist_mutex_.lock() ; + scan_hist_mutex_.lock(); // Determine where in our history we actually are - unsigned int i = 0 ; + unsigned int i = 0; - if ( scan_hist_.size() <= 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 + 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++ ; + i++; } - unsigned int start_index = i ; + unsigned int start_index = i; - unsigned int req_pts = 0 ; // Keep a total of the points in the current request + 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 + 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_ ; + req_pts += (scan_hist_[i].points.size() + downsample_factor_ - 1) / + downsample_factor_; + i += downsample_factor_; } - unsigned int past_end_index = i ; + unsigned int past_end_index = i; - if (start_index == past_end_index) - { - resp->cloud.header.frame_id = fixed_frame_ ; + 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 + 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; icloud.channels[i].name = scan_hist_[start_index].channels[i].name ; - resp->cloud.channels[i].values.resize (req_pts) ; + 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; icloud.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()) - ROS_FATAL("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 ()); + if (scan_hist_[i].points.size() != + scan_hist_[i].channels[chan_ind].values.size()) + { + ROS_FATAL("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; jcloud.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 ; + 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; kcloud.channels[k].values[cloud_count] = scan_hist_[i].channels[k].values[j] ; + 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++ ; + cloud_count++; } resp->cloud.header.stamp = scan_hist_[i].header.stamp; } } - scan_hist_mutex_.unlock() ; + scan_hist_mutex_.unlock(); - ROS_DEBUG("\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 ; + ROS_DEBUG("\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; } -template -bool BaseAssembler::buildCloud(std::shared_ptr req, std::shared_ptr resp) +template +bool BaseAssembler::buildCloud( + std::shared_ptr req, + std::shared_ptr + resp) { - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); + ROS_WARN( + "Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); return assembleScans(req, resp); } -template -bool BaseAssembler::assembleScans2(std::shared_ptr req, - std::shared_ptr resp) +template +bool BaseAssembler::assembleScans2( + std::shared_ptr req, + std::shared_ptr + resp) { std::shared_ptr tmp_req; - std::shared_ptr tmp_res; + std::shared_ptr + tmp_res; tmp_req->begin = req->begin; tmp_req->end = req->end; bool ret = assembleScans(tmp_req, tmp_res); - if ( ret ) - { + if (ret) { sensor_msgs::convertPointCloudToPointCloud2(tmp_res->cloud, resp->cloud); } return ret; @@ -424,14 +483,18 @@ bool BaseAssembler::assembleScans2(std::shared_ptr -bool BaseAssembler::buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) +bool +BaseAssembler::buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& +req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) { - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); - return assembleScans2(req, resp); + ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' +instead"); return assembleScans2(req, resp); } template -bool BaseAssembler::assembleScans2(laser_assembler_srv_gen::srv::AssembleScans2::Request& req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) +bool +BaseAssembler::assembleScans2(laser_assembler_srv_gen::srv::AssembleScans2::Request& +req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) { laser_assembler_srv_gen::srv::AssembleScans::Request tmp_req; laser_assembler_srv_gen::srv::AssembleScans::Response tmp_res; @@ -445,4 +508,5 @@ bool BaseAssembler::assembleScans2(laser_assembler_srv_gen::srv::AssembleScan } return ret; }*/ -} +} // namespace laser_assembler +#endif // LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ diff --git a/include/laser_assembler/base_assembler_srv.hpp b/include/laser_assembler/base_assembler_srv.hpp index 570d8d1..c584b81 100644 --- a/include/laser_assembler/base_assembler_srv.hpp +++ b/include/laser_assembler/base_assembler_srv.hpp @@ -1,54 +1,37 @@ -/********************************************************************* -* 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 +#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 "tf2_ros/message_filter.h" -#include "laser_assembler/message_filter.hpp" // TODO -#include "sensor_msgs/msg/point_cloud.hpp" +// #include "tf2_ros/message_filter.h" +#include "laser_assembler/message_filter.hpp" #include "message_filters/subscriber.h" - -#include +#include "sensor_msgs/msg/point_cloud.hpp" // Service #include "laser_assembler_srv_gen/srv/assemble_scans.hpp" -#include "boost/thread.hpp" -#include "math.h" - #define ROS_ERROR printf #define ROS_INFO printf #define ROS_WARN printf @@ -58,28 +41,33 @@ 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 + * \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. + * - \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 + * - \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() ; + BaseAssemblerSrv(); + ~BaseAssemblerSrv(); /** * \brief Tells the assembler to start listening to input data @@ -88,27 +76,30 @@ class BaseAssemblerSrv * create subcribes to the input stream, thus allowing the scanCallback and * ConvertToCloud to be called. Start should be called only once. */ - void start() ; - + 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 ; + 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 + * 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 ; + virtual void ConvertToCloud( + const std::string & fixed_frame_id, + const T & scan_in, + sensor_msgs::msg::PointCloud & cloud_out) = 0; protected: - tf::TransformListener* tf_ ; + tf::TransformListener * tf_; ros::NodeHandle private_ns_; ros::NodeHandle n_; @@ -117,220 +108,238 @@ class BaseAssemblerSrv // ROS Input/Ouptut Handling ros::ServiceServer cloud_srv_server_; message_filters::Subscriber scan_sub_; - tf::MessageFilter* tf_filter_; + 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) ; + // 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(assemble_scans::Request& req, assemble_scans::Response& resp) ; - + bool buildCloud(assemble_scans::Request & req, assemble_scans::Response & resp); //! \brief Stores history of scans - std::deque scan_hist_ ; - boost::mutex scan_hist_mutex_ ; + std::deque scan_hist_; + boost::mutex scan_hist_mutex_; //! \brief The number points currently in the scan history - unsigned int total_pts_ ; + unsigned int total_pts_; //! \brief The max number of scans to store in the scan history - unsigned int max_scans_ ; + 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_ ; + 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_ ; + //! \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_("~") +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) ; + 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) ; + ROS_INFO("TF Cache Time: %f Seconds", tf_cache_time_secs); // ***** Set max_scans ***** - const int default_max_scans = 400 ; - int tmp_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 ; + 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 + 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") ; + 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 ; + 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 ; + 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_) ; + 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); + 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_) ; + 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) + // (Well, don't start listening just yet. Keep this as null until we actually + // start listening, when start() is called) tf_filter_ = NULL; - } -template +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 - { + 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) ); + tf_filter_->registerCallback( + boost::bind(&BaseAssemblerSrv::scansCallback, this, _1)); } } -template +template BaseAssemblerSrv::~BaseAssemblerSrv() { - if (tf_filter_) + if (tf_filter_) { delete tf_filter_; + } - delete tf_ ; + delete tf_; } -template -void BaseAssemblerSrv::scansCallback(const boost::shared_ptr& scan_ptr) +template +void BaseAssemblerSrv::scansCallback( + const boost::shared_ptr & scan_ptr) { - const T scan = *scan_ptr ; + const T scan = *scan_ptr; - sensor_msgs::msg::PointCloud cur_cloud ; + 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) - { - ROS_WARN("Transform Exception %s", ex.what()) ; - return ; + 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_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 + 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_) ; + // printf("Scans: %4u Points: %10u\n", scan_hist_.size(), total_pts_) ; - scan_hist_mutex_.unlock() ; + scan_hist_mutex_.unlock(); } -template -bool BaseAssemblerSrv::buildCloud(assemble_scans::Request& req, assemble_scans::Response& resp) +template +bool BaseAssemblerSrv::buildCloud( + assemble_scans::Request & req, + assemble_scans::Response & resp) { - //printf("Starting Service Request\n") ; + // printf("Starting Service Request\n") ; - scan_hist_mutex_.lock() ; + scan_hist_mutex_.lock(); // Determine where in our history we actually are - unsigned int i = 0 ; + 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 + 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++ ; + i++; } - unsigned int start_index = i ; + unsigned int start_index = i; - unsigned int req_pts = 0 ; // Keep a total of the points in the current request + 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) + 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 { - resp.cloud.header.frame_id = fixed_frame_ ; - resp.cloud.header.stamp = req.end ; - resp.cloud.points.resize(0) ; - resp.cloud.channels.resize(0) ; + req_pts += (scan_hist_[i].points.size() + downsample_factor_ - 1) / + downsample_factor_; + i += downsample_factor_; } - else - { - // Note: We are assuming that channel information is consistent across multiple scans. If not, then bad things (segfaulting) will happen + 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 ; + ROS_DEBUG("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 index 5b0d02e..1adebf2 100644 --- a/include/laser_assembler/message_filter.hpp +++ b/include/laser_assembler/message_filter.hpp @@ -1,31 +1,17 @@ -/* - * Copyright (c) 2010, 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, Inc. 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 Josh Faust */ @@ -36,6 +22,8 @@ #include #include #include +#include +#include #include #include @@ -44,13 +32,19 @@ #include #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(), \ + 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(), \ + 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 @@ -60,9 +54,11 @@ 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. + /// 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 + /// 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, @@ -84,9 +80,12 @@ class MessageFilterBase }; /** - * \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 + * \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. + * 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. * @@ -100,33 +99,35 @@ class MessageFilterBase \endverbatim */ template -class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter +class MessageFilter : public MessageFilterBase, + public message_filters::SimpleFilter { public: using MConstPtr = std::shared_ptr; typedef message_filters::MessageEvent MEvent; - // typedef std::function FailureCallback; + // 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. + // 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 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) + 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(); @@ -136,19 +137,19 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi /** * \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 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) + 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(); @@ -157,15 +158,16 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi connectInput(f); } - /** - * \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. + * \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); + message_connection_ = + f.registerCallback(&MessageFilter::incomingMessage, this); } /** @@ -177,7 +179,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi clear(); TF2_ROS_MESSAGEFILTER_DEBUG( - "Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu", + "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_), @@ -186,7 +189,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } /** - * \brief Set the frame you need to be able to transform to before getting a message callback + * \brief Set the frame you need to be able to transform to before getting a + * message callback */ void setTargetFrame(const std::string & target_frame) { @@ -196,7 +200,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } /** - * \brief Set the frames you need to be able to transform to before getting a message callback + * \brief Set the frames you need to be able to transform to before getting a + * message callback */ void setTargetFrames(const V_string & target_frames) { @@ -205,10 +210,13 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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); + 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) { + for (V_string::iterator it = target_frames_.begin(); + it != target_frames_.end(); ++it) + { ss << *it << " "; } target_frames_string_ = ss.str(); @@ -230,7 +238,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi { std::unique_lock frames_lock(target_frames_mutex_); time_tolerance_ = tolerance; - expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); + expected_success_count_ = + target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); } /** @@ -243,13 +252,10 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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)); + 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; @@ -288,9 +294,9 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi V_string::iterator end = target_frames_copy.end(); for (; it != end; ++it) { const std::string & target_frame = *it; - tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, - target_frame, frame_id, tf2::timeFromSec( - stamp.seconds())); + tf2::TransformableRequestHandle handle = bc_.addTransformableRequest( + callback_handle_, target_frame, frame_id, + tf2::timeFromSec(stamp.seconds())); if (handle == 0xffffffffffffffffULL) { // never transformable messageDropped(evt, filter_failure_reasons::OutTheBack); @@ -302,8 +308,9 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } if (time_tolerance_.nanoseconds()) { - handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, tf2::timeFromSec( - (stamp + time_tolerance_).seconds())); + handle = bc_.addTransformableRequest( + callback_handle_, target_frame, frame_id, + tf2::timeFromSec((stamp + time_tolerance_).seconds())); if (handle == 0xffffffffffffffffULL) { // never transformable messageDropped(evt, filter_failure_reasons::OutTheBack); @@ -317,12 +324,12 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } } - // 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 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. @@ -331,7 +338,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi ++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)", + "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()).seconds()); @@ -354,33 +362,35 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi ++message_count_; } - TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", + TF2_ROS_MESSAGEFILTER_DEBUG( + "Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.seconds(), 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 + * \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); + std::shared_ptr> header( + new std::map); (*header)["callerid"] = "unknown"; - //Time t = rclcpp::Clock::now(); + // Time t = rclcpp::Clock::now(); rclcpp::Clock system_clock(RCL_SYSTEM_TIME); rclcpp::Time t = system_clock.now(); - //add(MEvent(message, header, t)); + // add(MEvent(message, header, t)); add(MEvent(message, t)); } /** - * \brief Register a callback to be called when a message is about to be dropped - * \param callback The callback to call + * \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) @@ -395,10 +405,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi queue_size_ = new_queue_size; } - virtual uint32_t getQueueSize() - { - return queue_size_; - } + virtual uint32_t getQueueSize() {return queue_size_;} private: void init() @@ -413,19 +420,17 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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)); + 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) + 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; @@ -434,8 +439,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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); + 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; @@ -464,14 +469,17 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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.seconds()))) { + if (!bc_.canTransform(target, frame_id, + tf2::timeFromSec(stamp.seconds()))) + { can_transform = false; break; } if (time_tolerance_.nanoseconds()) { - if (!bc_.canTransform(target, frame_id, - tf2::timeFromSec((stamp + time_tolerance_).seconds()))) + if (!bc_.canTransform( + target, frame_id, + tf2::timeFromSec((stamp + time_tolerance_).seconds()))) { can_transform = false; break; @@ -485,7 +493,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi // 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", + TF2_ROS_MESSAGEFILTER_DEBUG( + "Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.seconds(), message_count_ - 1); ++successful_transform_count_; @@ -493,7 +502,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi } else { ++dropped_message_count_; - TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", + TF2_ROS_MESSAGEFILTER_DEBUG( + "Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.seconds(), message_count_ - 1); messageDropped(info.event, filter_failure_reasons::Unknown); } @@ -521,85 +531,90 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi return; } - double dropped_pct = static_cast(dropped_message_count_) / + 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"); + 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) { + 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_.seconds(), last_out_the_back_frame_.c_str()); + " 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_.seconds(), + last_out_the_back_frame_.c_str()); } } } } - // TODO(clalancette): reenable this once we have underlying support for callback queues + // 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) + CBQueueCallback( + MessageFilter * filter, const MEvent & event, bool success, + FilterFailureReason reason) + : event_(event), + filter_(filter), + reason_(reason), + success_(success) {} virtual CallResult call() { - if (success_) - { + if (success_) { filter_->signalMessage(event_); - } - else - { + } else { filter_->signalFailure(event_, reason_); } return Success; } - private: +private: MEvent event_; - MessageFilter* filter_; + MessageFilter * filter_; FilterFailureReason reason_; bool success_; }; #endif - void messageDropped(const MEvent& evt, FilterFailureReason reason) + void messageDropped(const MEvent & evt, FilterFailureReason reason) { - // TODO(clalancette): reenable this once we have underlying support for callback queues + // 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 + } else #endif - { - signalFailure(evt, reason); - } + {signalFailure(evt, reason);} } void messageReady(const MEvent & evt) { - // TODO(clalancette): reenable this once we have underlying support for callback queues + // 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)); + ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, + filter_failure_reasons::Unknown)); callback_queue_->addCallback(cb, (uint64_t)this); - } - else + } else #endif - { - this->signalMessage(evt); - } + {this->signalMessage(evt);} } void signalFailure(const MEvent & evt, FilterFailureReason reason) @@ -608,13 +623,14 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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)", + RCLCPP_INFO(node_->get_logger(), + "[%s] Drop message: frame '%s' at time %.3f for reason(%d)", __func__, frame_id.c_str(), stamp.seconds(), reason); } static std::string stripSlash(const std::string & in) { - if (!in.empty() && (in [0] == '/')) { + if (!in.empty() && (in[0] == '/')) { std::string out = in; out.erase(0, 1); return out; @@ -629,13 +645,15 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi ///< 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. + ///< 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; + typedef std::vector + V_TransformableRequestHandle; struct MessageInfo { MessageInfo() @@ -648,7 +666,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi typedef std::list L_MessageInfo; L_MessageInfo messages_; - ///< The number of messages in the list. Used because \.size() may have linear cost + ///< 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_; @@ -667,13 +686,14 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi rclcpp::Time next_failure_warning_; - ///< Provide additional tolerance on time for messages which are stamped but can have associated duration + ///< 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 +} // namespace tf2_ros #endif diff --git a/include/laser_assembler/point_cloud_conversion.h b/include/laser_assembler/point_cloud_conversion.h deleted file mode 100644 index 5bebc44..0000000 --- a/include/laser_assembler/point_cloud_conversion.h +++ /dev/null @@ -1,169 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2010, 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 Willow Garage, Inc. 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. - * - * $Id$ - * - */ - -#ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_H -#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_H - -#include -#include -//#include -#include "point_field_conversion.h" - -/** - * \brief Convert between the old (sensor_msgs::PointCloud) and the new (sensor_msgs::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::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 ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)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 ((int)input.fields[d].offset == x_offset || (int)input.fields[d].offset == y_offset || (int)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); -} -} -#endif// SENSOR_MSGS_POINT_CLOUD_CONVERSION_H diff --git a/include/laser_assembler/point_cloud_conversion.hpp b/include/laser_assembler/point_cloud_conversion.hpp new file mode 100644 index 0000000..b3576b2 --- /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_ +#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ + +#include +#include +#include +// #include +#include "point_field_conversion.hpp" + +/** + * \brief Convert between the old (sensor_msgs::PointCloud) and the new + * (sensor_msgs::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::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 ((int)input.fields[d].offset == x_offset || + (int)input.fields[d].offset == y_offset || + (int)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 ((int)input.fields[d].offset == x_offset || + (int)input.fields[d].offset == y_offset || + (int)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_ diff --git a/include/laser_assembler/point_field_conversion.h b/include/laser_assembler/point_field_conversion.h deleted file mode 100644 index e9c2aad..0000000 --- a/include/laser_assembler/point_field_conversion.h +++ /dev/null @@ -1,180 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Robot Operating System code by the University of Osnabrück - * Copyright (c) 2015, University of Osnabrück - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above - * copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. 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. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED - * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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. - * - * - * - * point_field_conversion.h - * - * Created on: 16.07.2015 - * Authors: Sebastian Pütz - */ - -#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_H -#define SENSOR_MSGS_POINT_FIELD_CONVERSION_H - -/** - * \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; - } - } -} - -#endif /* point_field_conversion.h */ diff --git a/include/laser_assembler/point_field_conversion.hpp b/include/laser_assembler/point_field_conversion.hpp new file mode 100644 index 0000000..fe03dfd --- /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_ +#define SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ + +#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_ diff --git a/launch/test_laser_assembler.launch.py b/launch/test_laser_assembler.launch.py index 6565329..2251ed8 100644 --- a/launch/test_laser_assembler.launch.py +++ b/launch/test_laser_assembler.launch.py @@ -1,13 +1,26 @@ +#!/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. - -from launch import LaunchDescription -import launch_ros.actions import os -import yaml -from launch.substitutions import EnvironmentVariable import pathlib + +from launch import LaunchDescription import launch.actions -from launch.actions import DeclareLaunchArgument +from launch.substitutions import EnvironmentVariable +import launch_ros.actions def generate_launch_description(): @@ -16,30 +29,28 @@ def generate_launch_description(): 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' - ) - + 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'], - ], - ) - + 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' - ) + package='laser_assembler', node_executable='test_assembler', + output='screen' + ) return LaunchDescription([ launch.actions.DeclareLaunchArgument( @@ -53,16 +64,4 @@ def generate_launch_description(): target_action=test_laser_assembler, on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], )), -]) - - - - - - - - - - - - + ]) diff --git a/package.xml b/package.xml index 76c901d..8d1e4ba 100644 --- a/package.xml +++ b/package.xml @@ -38,6 +38,8 @@ builtin_interfaces laser_assembler_srv_gen + ament_lint_auto + ament_lint_common ament_cmake diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index 192bdb0..f34dccf 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -1,55 +1,31 @@ -/********************************************************************* -* 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 "laser_geometry/laser_geometry.hpp" - -//#include "laser_assembler/laser_geometry.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "laser_assembler/base_assembler.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include #include #include #include +#include #include +#include "laser_geometry/laser_geometry.hpp" + +// #include "laser_assembler/laser_geometry.hpp" #include "filters/filter_chain.h" +#include "laser_assembler/base_assembler.hpp" #include "rclcpp/time.hpp" - -using namespace laser_geometry; -using namespace std ; -using namespace std::chrono; +#include "sensor_msgs/msg/laser_scan.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #define ROS_ERROR printf #define ROS_INFO printf @@ -62,88 +38,94 @@ 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 { public: - LaserScanAssembler(rclcpp::Node::SharedPtr node_) : BaseAssembler("max_scans", node_), filter_chain_("sensor_msgs::msg::LaserScan") + explicit LaserScanAssembler(rclcpp::Node::SharedPtr node_) + : BaseAssembler("max_scans", node_), + filter_chain_("sensor_msgs::msg::LaserScan") { // ***** Set Laser Projection Method ***** - private_ns_->get_parameter_or("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_); - // 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_->create_subscription("scan", std::bind(&LaserScanAssembler::scanCallback, this, std::placeholders::_1)); + skew_scan_sub_ = n_->create_subscription( + "scan", std::bind(&LaserScanAssembler::scanCallback, this, + std::placeholders::_1)); } } - ~LaserScanAssembler() - { + ~LaserScanAssembler() {} - } - - unsigned int GetPointsInScan(const sensor_msgs::msg::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::msg::LaserScan& scan_in, sensor_msgs::msg::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_); int mask = laser_geometry::channel_option::Intensity + - laser_geometry::channel_option::Distance + - laser_geometry::channel_option::Index + - laser_geometry::channel_option::Timestamp; + laser_geometry::channel_option::Distance + + laser_geometry::channel_option::Index + + laser_geometry::channel_option::Timestamp; unsigned int n_pts = scan_in.ranges.size(); - - // TODO ignore_laser_skew_ this case is not handled right now, as there is no transformPointCloud support for PointCloud in ROS2. - 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 ROS2. + 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) { - - // TODO transform PointCloud + // TODO(vandana) transform PointCloud /* tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); TIME start_time = cloud_out.header.stamp; std::chrono::nanoseconds start(start_time.nanoseconds()); - std::chrono::time_point st(start); - geometry_msgs::msg::TransformStamped transform = tfBuffer.lookupTransform(fixed_frame_id, - cloud_out.header.frame_id, st); + std::chrono::time_point st(start); + geometry_msgs::msg::TransformStamped transform = + tfBuffer.lookupTransform(fixed_frame_id, cloud_out.header.frame_id, st); sensor_msgs::msg::PointCloud2 cloudout; tf2::doTransform(cloud_out, cloud_out, transform);*/ - } - } - else // Do it the slower (more accurate) way - { + } + } 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, tfBuffer, 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(std::shared_ptr laser_scan) + void + scanCallback(std::shared_ptr laser_scan) { - if (!ignore_laser_skew_) - { - rclcpp::Duration cur_tolerance = rclcpp::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_.nanoseconds()/1e+9, cur_tolerance.nanoseconds()/1e+9); + if (!ignore_laser_skew_) { + rclcpp::Duration cur_tolerance = rclcpp::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_.nanoseconds() / 1e+9, + cur_tolerance.nanoseconds() / 1e+9); assert(tf_filter_); tf_filter_->setTolerance(cur_tolerance); max_tolerance_ = cur_tolerance; @@ -157,22 +139,20 @@ class LaserScanAssembler : public BaseAssembler laser_geometry::LaserProjection projector_; 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 + 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_*/; - + 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) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("laser_scan_assembler"); - LaserScanAssembler pc_assembler(node); + 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 514df51..0362e8b 100644 --- a/src/laser_scan_assembler_srv.cpp +++ b/src/laser_scan_assembler_srv.cpp @@ -1,105 +1,88 @@ -/********************************************************************* -* 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.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" +// 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" #include "geometry_msgs/msg/transform_stamped.hpp"; #include "laser_assembler/base_assembler_srv.hpp" -#include "filters/filter_chain.h" +#include "laser_geometry/laser_geometry.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" #define ROS_ERROR printf #define ROS_INFO printf #define ROS_WARN printf #define ROS_DEBUG printf -using namespace laser_geometry; -using namespace std ; - 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::msg::LaserScan") + LaserScanAssemblerSrv() + : filter_chain_("sensor_msgs::msg::LaserScan") { // ***** Set Laser Projection Method ***** - private_ns_->get_parameter_or("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::msg::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::msg::LaserScan& scan_in, sensor_msgs::msg::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: @@ -108,21 +91,18 @@ class LaserScanAssemblerSrv : public BaseAssemblerSrv 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; + "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 5a142ff..4619be6 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 */ @@ -41,17 +21,17 @@ potentially from different sensors **/ // #include +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/point_cloud.hpp" -//#include -#include "laser_assembler/message_filter.hpp" // TODO message_filter.h file in ros2's tf2_ros package is not ported to ros2 yet. -#include - -#include -#include - -#include +#include "sensor_msgs/msg/point_cloud2.hpp" +// #include +#include "laser_assembler/message_filter.hpp" +#include "tf2_ros/transform_listener.h" +#include "message_filters/subscriber.h" #define ROS_ERROR printf #define ROS_INFO printf @@ -61,49 +41,50 @@ potentially from different sensors 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); 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(rclcpp::Duration(1.0/max_freq_), std::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 tf2_ros::MessageFilter(sub1_, tf_, output_frame_, 1)); - tf_filter2_.reset(new tf2_ros::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(std::bind(&MergeClouds::receiveCloud1, this, _1)); - tf_filter2_->registerCallback(std::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) @@ -115,32 +96,42 @@ class MergeClouds newCloud2_ = false; sensor_msgs::msg::PointCloud out; - if (cloud1_.header.stamp > cloud2_.header.stamp) + 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) { + // ROS_ASSERT(cloud1_.channels[i].values.size() == + // cloud1_.points.size()); ROS_ASSERT(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(); @@ -148,67 +139,69 @@ class MergeClouds cloudOut_.publish(out); } - void receiveCloud1(const sensor_msgs::msg::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::msg::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::msg::PointCloudConstPtr &cloud, sensor_msgs::msg::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::Timer timer_; + bool haveTimer_; - ros::Publisher cloudOut_; - double max_freq_; - std::string output_frame_; + ros::Publisher cloudOut_; + double max_freq_; + std::string output_frame_; message_filters::Subscriber sub1_; message_filters::Subscriber sub2_; - std::shared_ptr > tf_filter1_; - std::shared_ptr > tf_filter2_; + std::shared_ptr> tf_filter1_; + std::shared_ptr> tf_filter2_; - bool newCloud1_; - bool newCloud2_; + bool newCloud1_; + bool newCloud2_; sensor_msgs::msg::PointCloud cloud1_; sensor_msgs::msg::PointCloud cloud2_; - std::mutex lock1_; - std::mutex lock2_; - + std::mutex lock1_; + std::mutex lock2_; }; - -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("merge_clouds"); MergeClouds mc; mc.start("cloud"); rclcpp::spin(node); - - + return 0; } diff --git a/src/point_cloud2_assembler.cpp b/src/point_cloud2_assembler.cpp index 850a839..0e1a4ee 100644 --- a/src/point_cloud2_assembler.cpp +++ b/src/point_cloud2_assembler.cpp @@ -1,43 +1,21 @@ -/********************************************************************* -* 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 "laser_assembler/base_assembler.hpp" -//#include // TODO -#include "laser_assembler/point_cloud_conversion.h" - -using namespace std ; +// #include // TODO +#include "laser_assembler/point_cloud_conversion.hpp" #define ROS_ERROR printf #define ROS_INFO printf @@ -48,50 +26,45 @@ 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(rclcpp::Node::SharedPtr node) : BaseAssembler("max_clouds", node) - { - - } + explicit PointCloud2Assembler(rclcpp::Node::SharedPtr node) + : BaseAssembler("max_clouds", node) {} - ~PointCloud2Assembler() - { - - } + ~PointCloud2Assembler() {} - unsigned int GetPointsInScan(const sensor_msgs::msg::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::msg::PointCloud2& scan_in, sensor_msgs::msg::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::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) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("point_cloud_assembler"); - PointCloud2Assembler pc_assembler(node); + laser_assembler::PointCloud2Assembler pc_assembler(node); pc_assembler.start("cloud"); rclcpp::spin(node); diff --git a/src/point_cloud_assembler.cpp b/src/point_cloud_assembler.cpp index a61881b..ac72dc7 100644 --- a/src/point_cloud_assembler.cpp +++ b/src/point_cloud_assembler.cpp @@ -1,43 +1,20 @@ -/********************************************************************* -* 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 "laser_assembler/base_assembler.hpp" - -using namespace std; - #define ROS_ERROR printf #define ROS_INFO printf #define ROS_WARN printf @@ -47,50 +24,45 @@ 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 { public: - PointCloudAssembler(rclcpp::Node::SharedPtr node) : BaseAssembler("max_clouds", node) - { + 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::msg::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 std::string& fixed_frame_id, const sensor_msgs::msg::PointCloud& scan_in, sensor_msgs::msg::PointCloud& cloud_out) - { - // TODO - tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out) ; - //tf2::doTransform(scan_in, cloud_out, tfBuffer.lookupTransform(fixed_frame_id, tf2::getFrameId(scan_in), tf2::getTimestamp(scan_in))); - return ; + tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out); + // tf2::doTransform(scan_in, cloud_out, + // tfBuffer.lookupTransform(fixed_frame_id, tf2::getFrameId(scan_in), + // tf2::getTimestamp(scan_in))); } private: - }; -} - -using namespace laser_assembler ; +} // namespace laser_assembler -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = rclcpp::Node::make_shared("point_cloud_assembler"); - PointCloudAssembler pc_assembler(node); + laser_assembler::PointCloudAssembler pc_assembler(node); pc_assembler.start("cloud"); rclcpp::spin(node); diff --git a/src/point_cloud_assembler_srv.cpp b/src/point_cloud_assembler_srv.cpp index 4e94a40..fcc985e 100644 --- a/src/point_cloud_assembler_srv.cpp +++ b/src/point_cloud_assembler_srv.cpp @@ -1,42 +1,20 @@ -/********************************************************************* -* 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 "laser_assembler/base_assembler_srv.hpp" -using namespace std; - #define ROS_ERROR printf #define ROS_INFO printf #define ROS_WARN printf @@ -44,7 +22,6 @@ using namespace std; 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 @@ -56,43 +33,38 @@ class PointCloudAssemblerSrv : public BaseAssemblerSrvtransformPointCloud(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() ; + "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/test/dummy_scan_producer.cpp b/test/dummy_scan_producer.cpp index 60c5fc3..99ec7db 100644 --- a/test/dummy_scan_producer.cpp +++ b/test/dummy_scan_producer.cpp @@ -1,50 +1,31 @@ -/********************************************************************* -* 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 "rclcpp/rclcpp.hpp" -#include -#include -#include #include "tf2/LinearMath/Transform.h" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/transform_broadcaster.h" #define ROS_INFO printf @@ -52,12 +33,13 @@ void runLoop(rclcpp::Node::SharedPtr node_) { rclcpp::Rate loopRate(5); - - auto scan_pub = node_->create_publisher("dummy_scan", 100); + + auto scan_pub = + node_->create_publisher("dummy_scan", 100); // Configure the Transform broadcaster tf2_ros::TransformBroadcaster broadcaster(node_); - tf2::Transform laser_transform(tf2::Quaternion(0,0,0,1)); + tf2::Transform laser_transform(tf2::Quaternion(0, 0, 0, 1)); // Populate the dummy laser scan sensor_msgs::msg::LaserScan scan; @@ -74,19 +56,17 @@ void runLoop(rclcpp::Node::SharedPtr node_) scan.ranges.resize(N); scan.intensities.resize(N); - for (unsigned int i=0; ipublish(scan); - + geometry_msgs::msg::TransformStamped tf_transform; tf_transform.header.stamp = rclcpp::Clock().now(); tf_transform.header.frame_id = "dummy_laser_link"; @@ -98,11 +78,12 @@ void runLoop(rclcpp::Node::SharedPtr node_) } } -int main(int argc, char** argv) +int main(int argc, char ** argv) { printf("dummy scan producer\n"); rclcpp::init(argc, argv); - rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dummy_scan_producer"); + rclcpp::Node::SharedPtr node = + rclcpp::Node::make_shared("dummy_scan_producer"); std::thread run_thread(&runLoop, node); rclcpp::spin(node); run_thread.join(); diff --git a/test/test_assembler.cpp b/test/test_assembler.cpp index 5ac4e83..87bfc74 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -1,141 +1,121 @@ -/********************************************************************* -* 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 +#include +#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" #include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/point_cloud2.h" #include "sensor_msgs/msg/laser_scan.hpp" -#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" -#include +#include "sensor_msgs/msg/point_cloud2.h" +#include "gtest/gtest.h" #define ROS_INFO printf -using namespace sensor_msgs; -using namespace std; - -static const string SERVICE_NAME = "assemble_scans"; +// static const string SERVICE_NAME = "assemble_scans"; +static const char SERVICE_NAME[] = "assemble_scans"; class TestAssembler : public testing::Test { public: - rclcpp::Node::SharedPtr node; - //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); - rclcpp::Client::SharedPtr client_; - + // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + rclcpp::Client::SharedPtr + client_; + TestAssembler() {} - - static void spinThread(rclcpp::Node::SharedPtr node_) - { - rclcpp::spin(node_); - } + + static void spinThread(rclcpp::Node::SharedPtr node_) {rclcpp::spin(node_);} void SetUp() { - //ROS_INFO("Waiting for service [%s]", SERVICE_NAME.c_str()); + // ROS_INFO("Waiting for service [%s]", 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"); + client_ = node->create_client( + "assemble_scans"); + using namespace std::chrono_literals; if (!client_->wait_for_service(20s)) { printf("Service not available after waiting"); return; } printf(" Service assemble_scans started successfully"); - //ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); received_something_ = false; got_cloud_ = false; auto scan_callback = - [this](sensor_msgs::msg::LaserScan::ConstSharedPtr msg) -> void - { - this->ScanCallback(msg); - }; - scan_sub_ = node->create_subscription("dummy_scan", scan_callback); + [this](sensor_msgs::msg::LaserScan::ConstSharedPtr msg) -> void { + this->ScanCallback(msg); + }; + scan_sub_ = node->create_subscription( + "dummy_scan", scan_callback); } void ScanCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan_msg) { - std::unique_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 - + + using namespace std::chrono_literals; std::this_thread::sleep_for(0.2s); - auto request = std::make_shared(); + auto request = std::make_shared< + laser_assembler_srv_gen::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::SharedFuture; - + // 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_gen::srv::AssembleScans>::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { - auto result = future.get(); - printf("\nCloud points size = %ld\n", result.get()->cloud.points.size()); - //RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", future.get()->sum) - if (result.get()->cloud.points.size() > 0 ) { - - cloud_ = result.get()->cloud; - got_cloud_ = true; - cloud_condition_.notify_all(); - } - else - ROS_INFO("Got an empty cloud. Going to try again on the next scan"); - }; - - - - auto result = client_->async_send_request(request, response_received_callback); + auto result = future.get(); + printf("\nCloud points size = %ld\n", + result.get()->cloud.points.size()); + // RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", + // future.get()->sum) + if (result.get()->cloud.points.size() > 0) { + cloud_ = result.get()->cloud; + got_cloud_ = true; + cloud_condition_.notify_all(); + } else { + ROS_INFO("Got an empty cloud. Going to try again on the next scan"); + } + }; + + auto result = + client_->async_send_request(request, response_received_callback); } } @@ -143,34 +123,32 @@ class TestAssembler : public testing::Test rclcpp::Subscription::SharedPtr scan_sub_; bool received_something_; - + builtin_interfaces::msg::Time start_time_; bool got_cloud_; sensor_msgs::msg::PointCloud cloud_; - + std::mutex mutex_; - + 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 std::unique_lock lock(mutex_); - while(rclcpp::ok() && !got_cloud_) - { + 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"); From 176669ad451486f26ec913f1b2a096d694f054a4 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Thu, 20 Dec 2018 19:01:53 +0530 Subject: [PATCH 15/29] Added CONTRIBUTING.md and LICENSE file. --- CONTRIBUTING.md | 13 ++++ LICENSE | 202 ++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 215 insertions(+) create mode 100644 CONTRIBUTING.md create mode 100644 LICENSE 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. From d6c1b716a8ea6a4abca10d0ee36e3e3443a218de Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 21 Dec 2018 16:42:32 +0530 Subject: [PATCH 16/29] Replaced printf with ROS2 logging functions. --- examples/periodic_snapshotter.cpp | 35 +++-------- include/laser_assembler/base_assembler.hpp | 60 +++++++++---------- .../laser_assembler/base_assembler_srv.hpp | 33 +++++----- src/laser_scan_assembler.cpp | 11 +--- src/laser_scan_assembler_srv.cpp | 7 +-- src/merge_clouds.cpp | 5 -- src/point_cloud2_assembler.cpp | 5 -- src/point_cloud_assembler.cpp | 5 -- src/point_cloud_assembler_srv.cpp | 7 +-- test/dummy_scan_producer.cpp | 7 +-- test/test_assembler.cpp | 25 ++++---- 11 files changed, 64 insertions(+), 136 deletions(-) diff --git a/examples/periodic_snapshotter.cpp b/examples/periodic_snapshotter.cpp index 661a2f7..f3c7f99 100644 --- a/examples/periodic_snapshotter.cpp +++ b/examples/periodic_snapshotter.cpp @@ -27,12 +27,7 @@ #include "sensor_msgs/msg/point_cloud.hpp" #include "std_msgs/msg/string.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - -/*** +/** * This a simple test app that requests a point cloud from the * point_cloud_assembler every 4 seconds, and then publishes the * resulting data @@ -46,30 +41,19 @@ class PeriodicSnapshotter : public rclcpp::Node explicit PeriodicSnapshotter(const std::string & service_name) : Node(service_name) { - std::cerr << " Inside PeriodicSnapshotter constructor " << - "\n"; - + static const char SERVICE_NAME[] = "assemble_scans"; client_ = this->create_client( - "build_cloud"); + SERVICE_NAME); using namespace std::chrono_literals; if (!client_->wait_for_service(20s)) { - printf("Service not available after waiting"); + RCLCPP_INFO(this->get_logger(), "Service not available after waiting"); return; } - printf(" Service assemble_scans started successfully"); - // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + RCLCPP_INFO(this->get_logger(), "Service [%s] detected", SERVICE_NAME); // Create a function for when messages are to be sent. auto timer_callback = [this]() -> void { - // msg_->name = "Hello World: " + std::to_string(count_++); - // RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg_->status) - // RCLCPP_INFO(this->get_logger(), "Publishing: namespce %s and name is %s - // ", - // this->get_namespace(), this->get_name()); - - // Put the message into a queue to be processed by the middleware. - // This call is non-blocking. this->timerCallback(); }; @@ -111,16 +95,11 @@ class PeriodicSnapshotter : public rclcpp::Node auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get(); - printf( - "\n Cloud points size = %ld\n", - result.get()->cloud.points.size()); - // RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", - // future.get()->sum) if (result.get()->cloud.points.size() > 0) { - result.get()->cloud; + RCLCPP_INFO(this->get_logger(), "Got result: [ %ld ]", result.get()->cloud.points.size()); pub_->publish(result.get()->cloud); } else { - ROS_INFO("Got an empty cloud. Going to try again on the next scan"); + RCLCPP_INFO(this->get_logger(), "Got an empty cloud. Going to try again on the next scan"); } }; diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index 14bcbd6..4120133 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -46,12 +46,6 @@ #include "laser_assembler_srv_gen/srv/assemble_scans.hpp" #include "laser_assembler_srv_gen/srv/assemble_scans2.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf -#define ROS_FATAL printf - namespace laser_assembler { @@ -102,8 +96,8 @@ class BaseAssembler tf2::BufferCore tfBuffer; tf2_ros::TransformListener * tf_; tf2_ros::MessageFilter * tf_filter_; - rclcpp::Node::SharedPtr private_ns_; - rclcpp::Node * n_; + //rclcpp::Node::SharedPtr private_ns_; + rclcpp::Node::SharedPtr n_; private: // ROS Input/Ouptut Handling @@ -171,55 +165,55 @@ BaseAssembler::BaseAssembler( { // **** Initialize TransformListener **** double tf_cache_time_secs; - private_ns_ = node_; - n_ = node_.get(); + //private_ns_ = node_; + n_ = node_; - printf("BaseAssembler::BaseAssembler constructor "); + RCLCPP_INFO(n_->get_logger(), "BaseAssembler::BaseAssembler constructor "); - private_ns_->get_parameter_or("tf_cache_time_secs", tf_cache_time_secs, 10.0); + n_->get_parameter_or("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); + RCLCPP_ERROR(n_->get_logger(), "Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs); } tf_ = new tf2_ros::TransformListener(tfBuffer); - ROS_INFO("TF Cache Time: %f Seconds ", 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_->get_parameter_or(max_size_param_name, tmp_max_scans, + n_->get_parameter_or(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); + RCLCPP_ERROR(n_->get_logger(), "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_); + 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_->get_parameter_or("fixed_frame", fixed_frame_, + n_->get_parameter_or("fixed_frame", fixed_frame_, std::string("ERROR_NO_NAME")); - ROS_INFO("Fixed Frame: %s ", fixed_frame_.c_str()); + RCLCPP_INFO(n_->get_logger(),"Fixed Frame: %s ", fixed_frame_.c_str()); if (fixed_frame_ == "ERROR_NO_NAME") { - ROS_ERROR("Need to set parameter fixed_frame"); + RCLCPP_ERROR(n_->get_logger(), "Need to set parameter fixed_frame"); } // ***** Set downsample_factor ***** int tmp_downsample_factor; - private_ns_->get_parameter_or("downsample_factor", tmp_downsample_factor, 1); + n_->get_parameter_or("downsample_factor", tmp_downsample_factor, 1); if (tmp_downsample_factor < 1) { - ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor); + RCLCPP_ERROR(n_->get_logger(), "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 " + RCLCPP_WARN(n_->get_logger(), "Downsample set to [%u]. Note that this is an unreleased/unstable " "feature", downsample_factor_); } @@ -277,10 +271,10 @@ BaseAssembler::BaseAssembler( template void BaseAssembler::start(const std::string & in_topic_name) { - ROS_DEBUG("Called start(string). Starting to listen on " + RCLCPP_DEBUG(n_->get_logger(), "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 " + RCLCPP_ERROR(n_->get_logger(), "assembler::start() was called twice!. This is bad, and could " "leak memory"); } else { scan_sub_.subscribe(n_, in_topic_name); @@ -294,10 +288,10 @@ void BaseAssembler::start(const std::string & in_topic_name) template void BaseAssembler::start() { - ROS_DEBUG("Called start(). Starting tf::MessageFilter, but not initializing " + RCLCPP_DEBUG(n_->get_logger(), "Called start(). Starting tf::MessageFilter, but not initializing " "Subscriber"); if (tf_filter_) { - ROS_ERROR("assembler::start() was called twice!. This is bad, and could " + RCLCPP_ERROR(n_->get_logger(), "assembler::start() was called twice!. This is bad, and could " "leak memory"); } else { scan_sub_.subscribe(n_, "bogus"); @@ -321,7 +315,7 @@ BaseAssembler::~BaseAssembler() template void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) { - ROS_DEBUG("starting msgCallback"); + RCLCPP_DEBUG(n_->get_logger(), "starting msgCallback"); const T scan = *scan_ptr; sensor_msgs::msg::PointCloud cur_cloud; @@ -331,7 +325,7 @@ void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) ConvertToCloud(fixed_frame_, scan, cur_cloud); // Convert scan into a point cloud } catch (tf2::TransformException & ex) { - ROS_WARN("Transform Exception %s", ex.what()); + RCLCPP_WARN(n_->get_logger(), "Transform Exception %s", ex.what()); return; } @@ -352,7 +346,7 @@ void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) printf("Scans: %4lu Points: %10u\n", scan_hist_.size(), total_pts_); scan_hist_mutex_.unlock(); - ROS_DEBUG("done with msgCallback"); + RCLCPP_DEBUG(n_->get_logger(), "done with msgCallback"); } template @@ -418,7 +412,7 @@ bool BaseAssembler::assembleScans( if (scan_hist_[i].points.size() != scan_hist_[i].channels[chan_ind].values.size()) { - ROS_FATAL("Trying to add a malformed point cloud. Cloud has %u " + RCLCPP_FATAL(n_->get_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()); @@ -444,7 +438,7 @@ bool BaseAssembler::assembleScans( } scan_hist_mutex_.unlock(); - ROS_DEBUG("\nPoint Cloud Results: Aggregated from index %u->%u. BufferSize: " + RCLCPP_DEBUG(n_->get_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()); @@ -457,7 +451,7 @@ bool BaseAssembler::buildCloud( std::shared_ptr resp) { - ROS_WARN( + RCLCPP_WARN(n_->get_logger(), "Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); return assembleScans(req, resp); } diff --git a/include/laser_assembler/base_assembler_srv.hpp b/include/laser_assembler/base_assembler_srv.hpp index c584b81..9e9e1ef 100644 --- a/include/laser_assembler/base_assembler_srv.hpp +++ b/include/laser_assembler/base_assembler_srv.hpp @@ -32,11 +32,6 @@ // Service #include "laser_assembler_srv_gen/srv/assemble_scans.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - namespace laser_assembler { @@ -149,40 +144,40 @@ BaseAssemblerSrv::BaseAssemblerSrv() 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); + 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)); - ROS_INFO("TF Cache Time: %f Seconds", 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) { - ROS_ERROR("Parameter max_scans<0 (%i)", tmp_max_scans); + RCLCPP_ERROR(n_->get_logger(), "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_); + 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")); - ROS_INFO("Fixed Frame: %s", fixed_frame_.c_str()); + RCLCPP_INFO(n_->get_logger(), "Fixed Frame: %s", fixed_frame_.c_str()); if (fixed_frame_ == "ERROR_NO_NAME") { - ROS_ERROR("Need to set parameter fixed_frame"); + 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) { - ROS_ERROR("Parameter downsample_factor<1: %i", tmp_downsample_factor); + RCLCPP_ERROR(n_->get_logger(), "Parameter downsample_factor<1: %i", tmp_downsample_factor); tmp_downsample_factor = 1; } downsample_factor_ = tmp_downsample_factor; - ROS_INFO("Downsample Factor: %u", downsample_factor_); + RCLCPP_INFO(n_->get_logger(), "Downsample Factor: %u", downsample_factor_); // ***** Start Services ***** cloud_srv_server_ = private_ns_.advertiseService( @@ -191,9 +186,9 @@ BaseAssemblerSrv::BaseAssemblerSrv() // **** 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_); + RCLCPP_ERROR(n_->get_logger(), "Parameter tf_tolerance_secs<0 (%f)", tf_tolerance_secs_); } - ROS_INFO("tf Tolerance: %f seconds", 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 @@ -204,9 +199,9 @@ BaseAssemblerSrv::BaseAssemblerSrv() template void BaseAssemblerSrv::start() { - ROS_INFO("Starting to listen on the input stream"); + RCLCPP_INFO(n_->get_logger(), "Starting to listen on the input stream"); if (tf_filter_) { - ROS_ERROR("assembler::start() was called twice!. This is bad, and could " + 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); @@ -240,7 +235,7 @@ void BaseAssemblerSrv::scansCallback( ConvertToCloud(fixed_frame_, scan, cur_cloud); // Convert scan into a point cloud } catch (tf::TransformException & ex) { - ROS_WARN("Transform Exception %s", ex.what()); + RCLCPP_WARN(n_->get_logger(), "Transform Exception %s", ex.what()); return; } @@ -333,7 +328,7 @@ bool BaseAssemblerSrv::buildCloud( } scan_hist_mutex_.unlock(); - ROS_DEBUG("Point Cloud Results: Aggregated from index %u->%u. BufferSize: " + 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()); diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index f34dccf..3aea4e0 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -27,11 +27,6 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - #define TIME rclcpp::Time namespace laser_assembler @@ -49,11 +44,11 @@ class LaserScanAssembler : public BaseAssembler filter_chain_("sensor_msgs::msg::LaserScan") { // ***** Set Laser Projection Method ***** - private_ns_->get_parameter_or("ignore_laser_skew", ignore_laser_skew_, + 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", n_); // Have different callbacks, depending on whether or not we want to ignore // laser skews. @@ -123,7 +118,7 @@ class LaserScanAssembler : public BaseAssembler rclcpp::Duration cur_tolerance = rclcpp::Duration( laser_scan->time_increment * laser_scan->ranges.size()); if (cur_tolerance > max_tolerance_) { - ROS_DEBUG("Upping tf tolerance from [%.4fs] to [%.4fs]", + RCLCPP_DEBUG(n_->get_logger(), "Upping tf tolerance from [%.4fs] to [%.4fs]", max_tolerance_.nanoseconds() / 1e+9, cur_tolerance.nanoseconds() / 1e+9); assert(tf_filter_); diff --git a/src/laser_scan_assembler_srv.cpp b/src/laser_scan_assembler_srv.cpp index 0362e8b..7ed5bc0 100644 --- a/src/laser_scan_assembler_srv.cpp +++ b/src/laser_scan_assembler_srv.cpp @@ -20,11 +20,6 @@ #include "laser_geometry/laser_geometry.hpp" #include "sensor_msgs/msg/laser_scan.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - namespace laser_assembler { /** @@ -99,7 +94,7 @@ 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 " + 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; diff --git a/src/merge_clouds.cpp b/src/merge_clouds.cpp index 4619be6..c5fe785 100644 --- a/src/merge_clouds.cpp +++ b/src/merge_clouds.cpp @@ -33,11 +33,6 @@ potentially from different sensors #include "tf2_ros/transform_listener.h" #include "message_filters/subscriber.h" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - class MergeClouds { public: diff --git a/src/point_cloud2_assembler.cpp b/src/point_cloud2_assembler.cpp index 0e1a4ee..1b31112 100644 --- a/src/point_cloud2_assembler.cpp +++ b/src/point_cloud2_assembler.cpp @@ -17,11 +17,6 @@ // #include // TODO #include "laser_assembler/point_cloud_conversion.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - namespace laser_assembler { diff --git a/src/point_cloud_assembler.cpp b/src/point_cloud_assembler.cpp index ac72dc7..e5a0b4e 100644 --- a/src/point_cloud_assembler.cpp +++ b/src/point_cloud_assembler.cpp @@ -15,11 +15,6 @@ #include #include "laser_assembler/base_assembler.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - namespace laser_assembler { diff --git a/src/point_cloud_assembler_srv.cpp b/src/point_cloud_assembler_srv.cpp index fcc985e..72725dd 100644 --- a/src/point_cloud_assembler_srv.cpp +++ b/src/point_cloud_assembler_srv.cpp @@ -15,11 +15,6 @@ #include #include "laser_assembler/base_assembler_srv.hpp" -#define ROS_ERROR printf -#define ROS_INFO printf -#define ROS_WARN printf -#define ROS_DEBUG printf - namespace laser_assembler { /** @@ -60,7 +55,7 @@ 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 " + 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; diff --git a/test/dummy_scan_producer.cpp b/test/dummy_scan_producer.cpp index 99ec7db..6c7da1d 100644 --- a/test/dummy_scan_producer.cpp +++ b/test/dummy_scan_producer.cpp @@ -27,9 +27,6 @@ #include "tf2/LinearMath/Quaternion.h" #include "tf2_ros/transform_broadcaster.h" - -#define ROS_INFO printf - void runLoop(rclcpp::Node::SharedPtr node_) { rclcpp::Rate loopRate(5); @@ -64,7 +61,7 @@ void runLoop(rclcpp::Node::SharedPtr node_) // Keep sending scans until the assembler is done while (rclcpp::ok()) { scan.header.stamp = rclcpp::Clock().now(); - ROS_INFO("Publishing scan\n"); + RCLCPP_INFO(node_->get_logger(), "Publishing scan\n"); scan_pub->publish(scan); geometry_msgs::msg::TransformStamped tf_transform; @@ -74,13 +71,11 @@ void runLoop(rclcpp::Node::SharedPtr node_) tf_transform.transform.rotation.w = 1.0; broadcaster.sendTransform(tf_transform); loopRate.sleep(); - ROS_INFO("Publishing scan"); } } int main(int argc, char ** argv) { - printf("dummy scan producer\n"); rclcpp::init(argc, argv); rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("dummy_scan_producer"); diff --git a/test/test_assembler.cpp b/test/test_assembler.cpp index 87bfc74..8423e09 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -32,7 +32,7 @@ static const char SERVICE_NAME[] = "assemble_scans"; class TestAssembler : public testing::Test { public: - rclcpp::Node::SharedPtr node; + rclcpp::Node::SharedPtr node_; // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); rclcpp::Client::SharedPtr client_; @@ -43,19 +43,18 @@ class TestAssembler : public testing::Test void SetUp() { - // ROS_INFO("Waiting for service [%s]", SERVICE_NAME.c_str()); - node = rclcpp::Node::make_shared("test_assembler"); - std::thread spin_thread(spinThread, node); + node_ = rclcpp::Node::make_shared("test_assembler"); + std::thread spin_thread(spinThread, node_); spin_thread.detach(); - client_ = node->create_client( + 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)) { - printf("Service not available after waiting"); + RCLCPP_ERROR(node_->get_logger(), "Service not available after waiting"); return; } - printf(" Service assemble_scans started successfully"); - // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); + RCLCPP_INFO(node_->get_logger(), "Service assemble_scans started successfully"); received_something_ = false; got_cloud_ = false; @@ -63,7 +62,7 @@ class TestAssembler : public testing::Test [this](sensor_msgs::msg::LaserScan::ConstSharedPtr msg) -> void { this->ScanCallback(msg); }; - scan_sub_ = node->create_subscription( + scan_sub_ = node_->create_subscription( "dummy_scan", scan_callback); } @@ -101,16 +100,13 @@ class TestAssembler : public testing::Test auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get(); - printf("\nCloud points size = %ld\n", - result.get()->cloud.points.size()); - // RCLCPP_INFO(this->get_logger(), "Got result: [%" PRId64 "]", - // future.get()->sum) + 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 { - ROS_INFO("Got an empty cloud. Going to try again on the next scan"); + RCLCPP_INFO(node_->get_logger(), "Got an empty cloud. Going to try again on the next scan"); } }; @@ -151,7 +147,6 @@ TEST_F(TestAssembler, non_zero_cloud_test) { int main(int argc, char ** argv) { printf("******* Starting application *********\n"); - testing::InitGoogleTest(&argc, argv); rclcpp::init(0, nullptr); int result = RUN_ALL_TESTS(); From f8ce9ababe3445337ba1b821638543b81af66db5 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 21 Dec 2018 17:46:55 +0530 Subject: [PATCH 17/29] Code cleanup - Removed commented and unused code. --- include/laser_assembler/base_assembler.hpp | 114 +----------------- .../laser_assembler/base_assembler_srv.hpp | 3 - .../point_cloud_conversion.hpp | 6 +- src/laser_scan_assembler.cpp | 14 +-- src/merge_clouds.cpp | 6 +- src/point_cloud_assembler.cpp | 3 - test/test_assembler.cpp | 7 -- 7 files changed, 8 insertions(+), 145 deletions(-) diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index 4120133..8b57cb8 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -96,49 +96,25 @@ class BaseAssembler tf2::BufferCore tfBuffer; tf2_ros::TransformListener * tf_; tf2_ros::MessageFilter * tf_filter_; - //rclcpp::Node::SharedPtr private_ns_; rclcpp::Node::SharedPtr n_; private: // ROS Input/Ouptut Handling rclcpp::Service::SharedPtr assemble_scans_server_; - rclcpp::Service::SharedPtr - assemble_scans_server2_; - rclcpp::Service::SharedPtr - build_cloud_server_; - // rclcpp::Service::SharedPtr - // build_cloud_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 std::shared_ptr & scan_ptr); //! \brief Service Callback function called whenever we need to build a cloud - - bool buildCloud( - std::shared_ptr - request, - std::shared_ptr - response); - // bool assembleScans(laser_assembler_srv_gen::srv::AssembleScans::Request& - // req, laser_assembler_srv_gen::srv::AssembleScans::Response& resp); bool assembleScans( std::shared_ptr request, std::shared_ptr response); - bool assembleScans2( - std::shared_ptr - request, - std::shared_ptr - response); - // bool buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& - // req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp); //! \brief Stores history of scans std::deque scan_hist_; @@ -165,7 +141,6 @@ BaseAssembler::BaseAssembler( { // **** Initialize TransformListener **** double tf_cache_time_secs; - //private_ns_ = node_; n_ = node_; RCLCPP_INFO(n_->get_logger(), "BaseAssembler::BaseAssembler constructor "); @@ -233,35 +208,6 @@ BaseAssembler::BaseAssembler( n_->create_service( "assemble_scans", assembleScansCallback); - auto buildCloudCallback = - [this]( - std::shared_ptr - request, - std::shared_ptr - response) -> bool { - this->buildCloud(request, response); - return true; - }; - - build_cloud_server_ = - n_->create_service( - "build_cloud", buildCloudCallback); - - auto assembleScans2Callback = - [this]( - std::shared_ptr - request, - std::shared_ptr< - laser_assembler_srv_gen::srv::AssembleScans2::Response> - response) -> bool { - this->assembleScans2(request, response); - return true; - }; - - assemble_scans_server2_ = - n_->create_service( - "assemble_scans2", assembleScans2Callback); - // ***** Start Listening to Data ***** // (Well, don't start listening just yet. Keep this as null until we actually // start listening, when start() is called) @@ -343,7 +289,7 @@ void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) 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_); + //printf("Scans: %4lu Points: %10u\n", scan_hist_.size(), total_pts_); scan_hist_mutex_.unlock(); RCLCPP_DEBUG(n_->get_logger(), "done with msgCallback"); @@ -444,63 +390,5 @@ bool BaseAssembler::assembleScans( (int)resp->cloud.points.size()); return true; } - -template -bool BaseAssembler::buildCloud( - std::shared_ptr req, - std::shared_ptr - resp) -{ - RCLCPP_WARN(n_->get_logger(), - "Service 'build_cloud' is deprecated. Call 'assemble_scans' instead"); - return assembleScans(req, resp); -} - -template -bool BaseAssembler::assembleScans2( - std::shared_ptr req, - std::shared_ptr - resp) -{ - std::shared_ptr tmp_req; - std::shared_ptr - 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; -} - -/* -template -bool -BaseAssembler::buildCloud2(laser_assembler_srv_gen::srv::AssembleScans2::Request& -req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) -{ - ROS_WARN("Service 'build_cloud' is deprecated. Call 'assemble_scans' -instead"); return assembleScans2(req, resp); -} - -template -bool -BaseAssembler::assembleScans2(laser_assembler_srv_gen::srv::AssembleScans2::Request& -req, laser_assembler_srv_gen::srv::AssembleScans2::Response& resp) -{ - laser_assembler_srv_gen::srv::AssembleScans::Request tmp_req; - laser_assembler_srv_gen::srv::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; -}*/ } // namespace laser_assembler #endif // LASER_ASSEMBLER__BASE_ASSEMBLER_HPP_ diff --git a/include/laser_assembler/base_assembler_srv.hpp b/include/laser_assembler/base_assembler_srv.hpp index 9e9e1ef..37e0690 100644 --- a/include/laser_assembler/base_assembler_srv.hpp +++ b/include/laser_assembler/base_assembler_srv.hpp @@ -24,7 +24,6 @@ #include "rclcpp/rclcpp.hpp" #include "tf2_ros/transform_listener.h" -// #include "tf2_ros/message_filter.h" #include "laser_assembler/message_filter.hpp" #include "message_filters/subscriber.h" #include "sensor_msgs/msg/point_cloud.hpp" @@ -107,8 +106,6 @@ class BaseAssemblerSrv 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 diff --git a/include/laser_assembler/point_cloud_conversion.hpp b/include/laser_assembler/point_cloud_conversion.hpp index b3576b2..604b7a9 100644 --- a/include/laser_assembler/point_cloud_conversion.hpp +++ b/include/laser_assembler/point_cloud_conversion.hpp @@ -23,8 +23,8 @@ #include "point_field_conversion.hpp" /** - * \brief Convert between the old (sensor_msgs::PointCloud) and the new - * (sensor_msgs::PointCloud2) format. \author Radu Bogdan Rusu + * \brief Convert between the old (sensor_msgs::msg::PointCloud) and the new + * (sensor_msgs::msg::PointCloud2) format. \author Radu Bogdan Rusu */ namespace sensor_msgs { @@ -49,7 +49,7 @@ getPointCloud2FieldIndex( //////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /** \brief Convert a sensor_msgs::msg::PointCloud message to a - * sensor_msgs::PointCloud2 message. \param input the message in the + * 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 */ diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index 3aea4e0..a42690d 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -18,8 +18,6 @@ #include #include #include "laser_geometry/laser_geometry.hpp" - -// #include "laser_assembler/laser_geometry.hpp" #include "filters/filter_chain.h" #include "laser_assembler/base_assembler.hpp" #include "rclcpp/time.hpp" @@ -89,17 +87,9 @@ class LaserScanAssembler : public BaseAssembler 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) { + ; // TODO(vandana) transform PointCloud - /* - tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); - TIME start_time = cloud_out.header.stamp; - std::chrono::nanoseconds start(start_time.nanoseconds()); - std::chrono::time_point st(start); - geometry_msgs::msg::TransformStamped transform = - tfBuffer.lookupTransform(fixed_frame_id, cloud_out.header.frame_id, st); - sensor_msgs::msg::PointCloud2 cloudout; - tf2::doTransform(cloud_out, cloud_out, transform);*/ + // tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); } } else { // Do it the slower (more accurate) way int mask = laser_geometry::channel_option::Intensity + diff --git a/src/merge_clouds.cpp b/src/merge_clouds.cpp index c5fe785..7ba3d1e 100644 --- a/src/merge_clouds.cpp +++ b/src/merge_clouds.cpp @@ -28,7 +28,6 @@ potentially from different sensors #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -// #include #include "laser_assembler/message_filter.hpp" #include "tf2_ros/transform_listener.h" #include "message_filters/subscriber.h" @@ -108,9 +107,8 @@ class MergeClouds 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()); + 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; diff --git a/src/point_cloud_assembler.cpp b/src/point_cloud_assembler.cpp index e5a0b4e..857e315 100644 --- a/src/point_cloud_assembler.cpp +++ b/src/point_cloud_assembler.cpp @@ -43,9 +43,6 @@ class PointCloudAssembler : public BaseAssembler sensor_msgs::msg::PointCloud & cloud_out) { tf_->transformPointCloud(fixed_frame_id, scan_in, cloud_out); - // tf2::doTransform(scan_in, cloud_out, - // tfBuffer.lookupTransform(fixed_frame_id, tf2::getFrameId(scan_in), - // tf2::getTimestamp(scan_in))); } private: diff --git a/test/test_assembler.cpp b/test/test_assembler.cpp index 8423e09..82beccd 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -24,16 +24,12 @@ #include "sensor_msgs/msg/point_cloud2.h" #include "gtest/gtest.h" -#define ROS_INFO printf - -// static const string SERVICE_NAME = "assemble_scans"; static const char SERVICE_NAME[] = "assemble_scans"; class TestAssembler : public testing::Test { public: rclcpp::Node::SharedPtr node_; - // ROS_INFO("Service [%s] detected", SERVICE_NAME.c_str()); rclcpp::Client::SharedPtr client_; @@ -76,9 +72,6 @@ class TestAssembler : public testing::Test received_something_ = true; } else { // Make the call to get a point cloud - - using namespace std::chrono_literals; - std::this_thread::sleep_for(0.2s); auto request = std::make_shared< laser_assembler_srv_gen::srv::AssembleScans::Request>(); request->begin = start_time_.sec; From bd192640868cbd7cb10bcc864de13e90c71dd4a5 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 21 Dec 2018 17:52:17 +0530 Subject: [PATCH 18/29] Removed commented code from CMakeLists.txt. --- CMakeLists.txt | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f24b90..e13dc86 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,41 +44,18 @@ set(LIBS ${rclcpp_LIBRARIES} ${ament_cmake_LIBRARIES} ${message_filters_LIBRARIES} ${sensor_msgs_LIBRARIES} ${filters_LIBRARIES}) -# add_executable(laser_scan_assembler_srv src/laser_scan_assembler_srv.cpp) -# target_link_libraries(laser_scan_assembler_srv ${LIBS} ${Boost_LIBRARIES}) - add_executable(laser_scan_assembler src/laser_scan_assembler.cpp) target_link_libraries(laser_scan_assembler ${LIBS}) install( TARGETS laser_scan_assembler DESTINATION lib/${PROJECT_NAME}) -#add_executable(merge_clouds src/merge_clouds.cpp) -#target_link_libraries(merge_clouds ${LIBS}) - -# add_executable(point_cloud_assembler_srv src/point_cloud_assembler_srv.cpp) -# target_link_libraries(point_cloud_assembler_srv ${LIBS}) - -#add_executable(point_cloud_assembler src/point_cloud_assembler.cpp) -#target_link_libraries(point_cloud_assembler ${LIBS}) -#install( -# TARGETS point_cloud_assembler -# DESTINATION lib/${PROJECT_NAME}) - -# add_executable(point_cloud2_assembler src/point_cloud2_assembler.cpp) -# target_link_libraries(point_cloud2_assembler ${LIBS}) -# install( - #TARGETS point_cloud2_assembler - #DESTINATION lib/${PROJECT_NAME}) - include_directories(include) install(DIRECTORY "include/" DESTINATION include) install(TARGETS laser_scan_assembler - #point_cloud_assembler - #point_cloud2_assembler DESTINATION lib/${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -89,7 +66,6 @@ ament_export_include_directories(include) ament_export_dependencies(rclcpp) ament_export_dependencies(signals) ament_export_dependencies(sensor_msgs) -#ament_export_dependencies(laser_geometry) ament_export_dependencies(tf2_ros) ament_export_dependencies(tf2) ament_export_dependencies(message_filters) From 081d2d79dbc65b9ce922bc7187e4a165b14637f2 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Thu, 27 Dec 2018 18:15:10 +0530 Subject: [PATCH 19/29] - Applied linter (cpp_lint, uncrustify, lint_cmake etc) for code format. - Fixed logger issue for bouncy. - Modified message_filter.hpp to work on bouncy. --- examples/periodic_snapshotter.cpp | 3 +- include/laser_assembler/base_assembler.hpp | 48 +-- include/laser_assembler/message_filter.hpp | 321 +++++++++--------- .../point_cloud_conversion.hpp | 22 +- .../point_field_conversion.hpp | 6 +- src/laser_scan_assembler.cpp | 10 +- test/test_assembler.cpp | 6 +- 7 files changed, 206 insertions(+), 210 deletions(-) diff --git a/examples/periodic_snapshotter.cpp b/examples/periodic_snapshotter.cpp index f3c7f99..74bb301 100644 --- a/examples/periodic_snapshotter.cpp +++ b/examples/periodic_snapshotter.cpp @@ -99,7 +99,8 @@ class PeriodicSnapshotter : public rclcpp::Node 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"); + RCLCPP_INFO(this->get_logger(), + "Got an empty cloud. Going to try again on the next scan"); } }; diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index 8b57cb8..0216991 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -46,6 +46,8 @@ #include "laser_assembler_srv_gen/srv/assemble_scans.hpp" #include "laser_assembler_srv_gen/srv/assemble_scans2.hpp" +rclcpp::Logger g_logger = rclcpp::get_logger("laser_scan_assembler"); + namespace laser_assembler { @@ -59,7 +61,7 @@ class BaseAssembler public: BaseAssembler( const std::string & max_size_param_name, - rclcpp::Node::SharedPtr node_); + const rclcpp::Node::SharedPtr & node_); ~BaseAssembler(); /** @@ -93,10 +95,10 @@ class BaseAssembler sensor_msgs::msg::PointCloud & cloud_out) = 0; protected: + rclcpp::Node * n_; tf2::BufferCore tfBuffer; tf2_ros::TransformListener * tf_; tf2_ros::MessageFilter * tf_filter_; - rclcpp::Node::SharedPtr n_; private: // ROS Input/Ouptut Handling @@ -137,22 +139,20 @@ class BaseAssembler template BaseAssembler::BaseAssembler( const std::string & max_size_param_name, - rclcpp::Node::SharedPtr node_) + const rclcpp::Node::SharedPtr & node_) { // **** Initialize TransformListener **** double tf_cache_time_secs; - n_ = node_; - - RCLCPP_INFO(n_->get_logger(), "BaseAssembler::BaseAssembler constructor "); + n_ = node_.get(); n_->get_parameter_or("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); + RCLCPP_ERROR(g_logger, "Parameter tf_cache_time_secs<0 (%f)", tf_cache_time_secs); } tf_ = new tf2_ros::TransformListener(tfBuffer); - RCLCPP_INFO(n_->get_logger(), "TF Cache Time: %f Seconds ", tf_cache_time_secs); + RCLCPP_INFO(g_logger, "TF Cache Time: %f Seconds ", tf_cache_time_secs); // ***** Set max_scans ***** const int default_max_scans = 400; @@ -162,33 +162,33 @@ BaseAssembler::BaseAssembler( default_max_scans); if (tmp_max_scans < 0) { - RCLCPP_ERROR(n_->get_logger(), "Parameter max_scans<0 (%i)", tmp_max_scans); + 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(n_->get_logger(), "Max Scans in History: %u ", 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(n_->get_logger(),"Fixed Frame: %s ", fixed_frame_.c_str()); + RCLCPP_INFO(g_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"); + 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(n_->get_logger(), "Parameter downsample_factor<1: %i", tmp_downsample_factor); + 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(n_->get_logger(), "Downsample set to [%u]. Note that this is an unreleased/unstable " + RCLCPP_WARN(g_logger, "Downsample set to [%u]. Note that this is an unreleased/unstable " "feature", downsample_factor_); } @@ -217,10 +217,10 @@ BaseAssembler::BaseAssembler( template void BaseAssembler::start(const std::string & in_topic_name) { - RCLCPP_DEBUG(n_->get_logger(), "Called start(string). Starting to listen on " + RCLCPP_DEBUG(g_logger, "Called start(string). Starting to listen on " "message_filter::Subscriber the input stream"); if (tf_filter_) { - RCLCPP_ERROR(n_->get_logger(), "assembler::start() was called twice!. This is bad, and could " + RCLCPP_ERROR(g_logger, "assembler::start() was called twice!. This is bad, and could " "leak memory"); } else { scan_sub_.subscribe(n_, in_topic_name); @@ -234,10 +234,10 @@ void BaseAssembler::start(const std::string & in_topic_name) template void BaseAssembler::start() { - RCLCPP_DEBUG(n_->get_logger(), "Called start(). Starting tf::MessageFilter, but not initializing " + RCLCPP_DEBUG(g_logger, "Called start(). Starting tf::MessageFilter, but not initializing " "Subscriber"); if (tf_filter_) { - RCLCPP_ERROR(n_->get_logger(), "assembler::start() was called twice!. This is bad, and could " + RCLCPP_ERROR(g_logger, "assembler::start() was called twice!. This is bad, and could " "leak memory"); } else { scan_sub_.subscribe(n_, "bogus"); @@ -261,7 +261,7 @@ BaseAssembler::~BaseAssembler() template void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) { - RCLCPP_DEBUG(n_->get_logger(), "starting msgCallback"); + RCLCPP_DEBUG(g_logger, "starting msgCallback"); const T scan = *scan_ptr; sensor_msgs::msg::PointCloud cur_cloud; @@ -271,7 +271,7 @@ void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) ConvertToCloud(fixed_frame_, scan, cur_cloud); // Convert scan into a point cloud } catch (tf2::TransformException & ex) { - RCLCPP_WARN(n_->get_logger(), "Transform Exception %s", ex.what()); + RCLCPP_WARN(g_logger, "Transform Exception %s", ex.what()); return; } @@ -289,10 +289,10 @@ void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) 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_); + // printf("Scans: %4lu Points: %10u\n", scan_hist_.size(), total_pts_); scan_hist_mutex_.unlock(); - RCLCPP_DEBUG(n_->get_logger(), "done with msgCallback"); + RCLCPP_DEBUG(g_logger, "done with msgCallback"); } template @@ -358,7 +358,7 @@ bool BaseAssembler::assembleScans( if (scan_hist_[i].points.size() != scan_hist_[i].channels[chan_ind].values.size()) { - RCLCPP_FATAL(n_->get_logger(), "Trying to add a malformed point cloud. Cloud has %u " + 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()); @@ -384,7 +384,7 @@ bool BaseAssembler::assembleScans( } scan_hist_mutex_.unlock(); - RCLCPP_DEBUG(n_->get_logger(), "\nPoint Cloud Results: Aggregated from index %u->%u. BufferSize: " + 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()); diff --git a/include/laser_assembler/message_filter.hpp b/include/laser_assembler/message_filter.hpp index 1adebf2..7ff0b17 100644 --- a/include/laser_assembler/message_filter.hpp +++ b/include/laser_assembler/message_filter.hpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. - /** \author Josh Faust */ -#ifndef TF2_ROS_MESSAGE_FILTER_H -#define TF2_ROS_MESSAGE_FILTER_H +#ifndef TF2_ROS_MESSAGE_FILTER_H // NOLINT +#define TF2_ROS_MESSAGE_FILTER_H // NOLINT #include #include @@ -25,26 +24,20 @@ #include #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(), \ + 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(), \ + 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 @@ -54,16 +47,17 @@ 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. + /// 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 + /// 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; @@ -71,7 +65,6 @@ class MessageFilterBase { public: typedef std::vector V_string; - virtual ~MessageFilterBase() {} virtual void clear() = 0; virtual void setTargetFrame(const std::string & target_frame) = 0; @@ -80,12 +73,12 @@ class MessageFilterBase }; /** - * \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 + * \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. + * 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. * @@ -99,19 +92,18 @@ class MessageFilterBase \endverbatim */ template -class MessageFilter : public MessageFilterBase, - public message_filters::SimpleFilter +class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter { public: using MConstPtr = std::shared_ptr; typedef message_filters::MessageEvent MEvent; - // typedef std::function - // FailureCallback; + // 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. + // 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); /** @@ -119,15 +111,18 @@ class MessageFilter : public MessageFilterBase, * * \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). + * 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) + 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(); @@ -137,19 +132,22 @@ class MessageFilter : public MessageFilterBase, /** * \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 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) + 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(); @@ -159,15 +157,14 @@ class MessageFilter : public MessageFilterBase, } /** - * \brief Connect this filter's input to another filter's output. If this - * filter is already connected, disconnects first. + * \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); + message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this); } /** @@ -181,16 +178,16 @@ class MessageFilter : public MessageFilterBase, 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_)); + 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 + * \brief Set the frame you need to be able to transform + * to before getting a message callback */ void setTargetFrame(const std::string & target_frame) { @@ -200,8 +197,8 @@ class MessageFilter : public MessageFilterBase, } /** - * \brief Set the frames you need to be able to transform to before getting a - * message callback + * \brief Set the frames you need to be able to transform + * to before getting a message callback */ void setTargetFrames(const V_string & target_frames) { @@ -210,13 +207,10 @@ class MessageFilter : public MessageFilterBase, 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); + 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) - { + for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) { ss << *it << " "; } target_frames_string_ = ss.str(); @@ -238,8 +232,7 @@ class MessageFilter : public MessageFilterBase, { std::unique_lock frames_lock(target_frames_mutex_); time_tolerance_ = tolerance; - expected_success_count_ = - target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); + expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1); } /** @@ -252,10 +245,13 @@ class MessageFilter : public MessageFilterBase, 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)); + 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; @@ -279,7 +275,8 @@ class MessageFilter : public MessageFilterBase, return; } - // iterate through the target frames and add requests for each of them + // iterate through the target frames + // and add requests for each of them MessageInfo info; info.handles.reserve(expected_success_count_); { @@ -294,9 +291,9 @@ class MessageFilter : public MessageFilterBase, V_string::iterator end = target_frames_copy.end(); for (; it != end; ++it) { const std::string & target_frame = *it; - tf2::TransformableRequestHandle handle = bc_.addTransformableRequest( - callback_handle_, target_frame, frame_id, - tf2::timeFromSec(stamp.seconds())); + 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); @@ -308,9 +305,9 @@ class MessageFilter : public MessageFilterBase, } if (time_tolerance_.nanoseconds()) { - handle = bc_.addTransformableRequest( - callback_handle_, target_frame, frame_id, - tf2::timeFromSec((stamp + time_tolerance_).seconds())); + 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); @@ -328,21 +325,19 @@ class MessageFilter : public MessageFilterBase, 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 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)", + "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()).seconds()); + (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(); @@ -362,38 +357,39 @@ class MessageFilter : public MessageFilterBase, ++message_count_; } - TF2_ROS_MESSAGEFILTER_DEBUG( - "Added message in frame %s at time %.3f, count now %d", - frame_id.c_str(), stamp.seconds(), 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 + * \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); + 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)); - add(MEvent(message, t)); + 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 + * \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_filters::Connection + registerFailureCallback(const FailureCallback & callback) { message_connection_failure.disconnect(); message_connection_failure = this->registerCallback(callback, this); @@ -405,7 +401,10 @@ class MessageFilter : public MessageFilterBase, queue_size_ = new_queue_size; } - virtual uint32_t getQueueSize() {return queue_size_;} + virtual uint32_t getQueueSize() + { + return queue_size_; + } private: void init() @@ -420,17 +419,19 @@ class MessageFilter : public MessageFilterBase, 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)); + 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) + 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; @@ -439,8 +440,8 @@ class MessageFilter : public MessageFilterBase, 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); + 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; @@ -469,17 +470,14 @@ class MessageFilter : public MessageFilterBase, 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.seconds()))) - { + 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_).seconds()))) + if (!bc_.canTransform(target, frame_id, + tf2::timeFromSec((stamp + time_tolerance_).nanoseconds() / 1e+9))) { can_transform = false; break; @@ -493,18 +491,16 @@ class MessageFilter : public MessageFilterBase, // 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.seconds(), message_count_ - 1); + 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.seconds(), message_count_ - 1); + 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); } @@ -531,33 +527,30 @@ class MessageFilter : public MessageFilterBase, return; } - double dropped_pct = - static_cast(dropped_message_count_) / + 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"); + 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) + 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_.seconds(), - last_out_the_back_frame_.c_str()); + " 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 + // TODO(clalancette): reenable this once we have underlying support for callback queues #if 0 struct CBQueueCallback : public ros::CallbackInterface { @@ -592,29 +585,31 @@ class MessageFilter : public MessageFilterBase, void messageDropped(const MEvent & evt, FilterFailureReason reason) { - // TODO(clalancette): reenable this once we have underlying support for - // callback queues + // 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 + } else { + signalFailure(evt, reason); + } #endif - {signalFailure(evt, reason);} + signalFailure(evt, reason); } void messageReady(const MEvent & evt) { - // TODO(clalancette): reenable this once we have underlying support for - // callback queues + // 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 + } else { + this->signalMessage(evt); + } #endif - {this->signalMessage(evt);} + this->signalMessage(evt); } void signalFailure(const MEvent & evt, FilterFailureReason reason) @@ -623,9 +618,9 @@ class MessageFilter : public MessageFilterBase, 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.seconds(), reason); + // 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) @@ -640,20 +635,18 @@ class MessageFilter : public MessageFilterBase, } const rclcpp::Node::SharedPtr node_; - ///< The Transformer used to determine if transformation data is available + /// < 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 + /// < 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. + /// < 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 + /// < The maximum number of messages we queue up uint32_t queue_size_; tf2::TransformableCallbackHandle callback_handle_; - typedef std::vector - V_TransformableRequestHandle; + typedef std::vector V_TransformableRequestHandle; struct MessageInfo { MessageInfo() @@ -666,10 +659,10 @@ class MessageFilter : public MessageFilterBase, typedef std::list L_MessageInfo; L_MessageInfo messages_; - ///< The number of messages in the list. Used because \.size() - ///< may have linear cost + /// < 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 + /// < The mutex used for locking message list operations std::mutex messages_mutex_; uint64_t expected_success_count_; @@ -686,8 +679,8 @@ class MessageFilter : public MessageFilterBase, rclcpp::Time next_failure_warning_; - ///< Provide additional tolerance on time for messages which are stamped but - ///< can have associated duration + /// < 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_; @@ -696,4 +689,4 @@ class MessageFilter : public MessageFilterBase, } // namespace tf2_ros -#endif +#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 index 604b7a9..721c28d 100644 --- a/include/laser_assembler/point_cloud_conversion.hpp +++ b/include/laser_assembler/point_cloud_conversion.hpp @@ -13,12 +13,12 @@ // limitations under the License. -#ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ -#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ +#ifndef SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ // NOLINT +#define SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ // NOLINT #include -#include -#include +#include "sensor_msgs/msg/point_cloud.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" // #include #include "point_field_conversion.hpp" @@ -133,9 +133,9 @@ convertPointCloud2ToPointCloud( // Convert the fields to channels int cur_c = 0; for (size_t d = 0; d < input.fields.size(); ++d) { - if ((int)input.fields[d].offset == x_offset || - (int)input.fields[d].offset == y_offset || - (int)input.fields[d].offset == z_offset) + 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; } @@ -156,9 +156,9 @@ convertPointCloud2ToPointCloud( // Copy the rest of the data int cur_c = 0; for (size_t d = 0; d < input.fields.size(); ++d) { - if ((int)input.fields[d].offset == x_offset || - (int)input.fields[d].offset == y_offset || - (int)input.fields[d].offset == z_offset) + 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; } @@ -171,4 +171,4 @@ convertPointCloud2ToPointCloud( return true; } } // namespace sensor_msgs -#endif // SENSOR_MSGS_POINT_CLOUD_CONVERSION_HPP_ +#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 index fe03dfd..c34d0eb 100644 --- a/include/laser_assembler/point_field_conversion.hpp +++ b/include/laser_assembler/point_field_conversion.hpp @@ -17,8 +17,8 @@ // Created on: 16.07.2015 // Authors: Sebastian Pütz -#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ -#define SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ +#ifndef SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ // NOLINT +#define SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ // NOLINT #include /** @@ -241,4 +241,4 @@ inline void writePointCloud2BufferValue( } } // namespace sensor_msgs -#endif // SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ +#endif // SENSOR_MSGS_POINT_FIELD_CONVERSION_HPP_ // NOLINT diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index a42690d..e654fe8 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -37,7 +37,7 @@ namespace laser_assembler class LaserScanAssembler : public BaseAssembler { public: - explicit LaserScanAssembler(rclcpp::Node::SharedPtr node_) + explicit LaserScanAssembler(const rclcpp::Node::SharedPtr & node_) : BaseAssembler("max_scans", node_), filter_chain_("sensor_msgs::msg::LaserScan") { @@ -46,7 +46,7 @@ class LaserScanAssembler : public BaseAssembler true); // configure the filter chain from the parameter server - filter_chain_.configure("filters", n_); + filter_chain_.configure("filters", node_); // Have different callbacks, depending on whether or not we want to ignore // laser skews. @@ -87,7 +87,6 @@ class LaserScanAssembler : public BaseAssembler 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) { - ; // TODO(vandana) transform PointCloud // tf_->transformPointCloud(fixed_frame_id, cloud_out, cloud_out); } @@ -108,7 +107,7 @@ class LaserScanAssembler : public BaseAssembler rclcpp::Duration cur_tolerance = rclcpp::Duration( laser_scan->time_increment * laser_scan->ranges.size()); if (cur_tolerance > max_tolerance_) { - RCLCPP_DEBUG(n_->get_logger(), "Upping tf tolerance from [%.4fs] to [%.4fs]", + RCLCPP_DEBUG(g_logger, "Upping tf tolerance from [%.4fs] to [%.4fs]", max_tolerance_.nanoseconds() / 1e+9, cur_tolerance.nanoseconds() / 1e+9); assert(tf_filter_); @@ -136,7 +135,8 @@ class LaserScanAssembler : public BaseAssembler int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("laser_scan_assembler"); + 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); diff --git a/test/test_assembler.cpp b/test/test_assembler.cpp index 82beccd..cd9eaa8 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -93,13 +93,15 @@ class TestAssembler : public testing::Test auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get(); - RCLCPP_INFO(node_->get_logger(), "Got result: [ %ld ]", result.get()->cloud.points.size()); + 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"); + RCLCPP_INFO( + node_->get_logger(), "Got an empty cloud. Going to try again on the next scan"); } }; From 64ed3c83849635272463e0d01081dcdab0cc5365 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 28 Dec 2018 11:46:19 +0530 Subject: [PATCH 20/29] Added Ros2 migration read me file. --- ros2_migration_readme.md | 91 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 91 insertions(+) create mode 100644 ros2_migration_readme.md diff --git a/ros2_migration_readme.md b/ros2_migration_readme.md new file mode 100644 index 0000000..40da3e8 --- /dev/null +++ b/ros2_migration_readme.md @@ -0,0 +1,91 @@ +This file describes the work done and steps to perform test on the laser_assembler package. + +"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 + git clone https://github.com/ros2/launch/tree/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 + remove laser_assembler_srv_gen folder from laser_assembler and paste it in src folder (so your src folder should contain laser_assembler and laser_assembler_srv_gen directories.) + 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. + From 17baa16ea0a2137ae287cedfd5c6b6b20fb6ae3a Mon Sep 17 00:00:00 2001 From: vandanamandlik <43747373+vandanamandlik@users.noreply.github.com> Date: Fri, 28 Dec 2018 11:52:38 +0530 Subject: [PATCH 21/29] Formatted ros2_migration_readme.md file --- ros2_migration_readme.md | 1 + 1 file changed, 1 insertion(+) diff --git a/ros2_migration_readme.md b/ros2_migration_readme.md index 40da3e8..44097db 100644 --- a/ros2_migration_readme.md +++ b/ros2_migration_readme.md @@ -87,5 +87,6 @@ Do the 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. From c885af4a86d02e8bcd4e060c1f6ed7babff105b3 Mon Sep 17 00:00:00 2001 From: vandanamandlik <43747373+vandanamandlik@users.noreply.github.com> Date: Fri, 28 Dec 2018 11:57:14 +0530 Subject: [PATCH 22/29] modified Ros2_migration_readme file. --- ros2_migration_readme.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ros2_migration_readme.md b/ros2_migration_readme.md index 44097db..7df8139 100644 --- a/ros2_migration_readme.md +++ b/ros2_migration_readme.md @@ -4,6 +4,7 @@ This file describes the work done and steps to perform test on the laser_assembl 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. @@ -13,6 +14,7 @@ ROS2 Migration changes Migrated CMakeLists.txt and package.xml in ROS2 style Dependencies + filters laser_geometry message_filters From 67771c826ae8c6e04e1202d7775d20879f51a633 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 28 Dec 2018 13:58:43 +0530 Subject: [PATCH 23/29] Added ament_export_dependencies for laser_geometry package in CMakeLists.txt file. --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index e13dc86..82831d6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ 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 From 4eebfab8c2568bf1485d068ff57b0b5e21940e1e Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 28 Dec 2018 15:35:09 +0530 Subject: [PATCH 24/29] Added comments in source code and removed some unused code. --- include/laser_assembler/base_assembler.hpp | 5 +++-- include/laser_assembler/message_filter.hpp | 2 ++ src/laser_scan_assembler.cpp | 3 +-- src/merge_clouds.cpp | 1 - 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index 0216991..f3f3364 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -28,7 +28,7 @@ #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 ros2's +// #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 @@ -95,6 +95,7 @@ class BaseAssembler sensor_msgs::msg::PointCloud & cloud_out) = 0; protected: + // message_filters's subscribe method requires raw node pointer. rclcpp::Node * n_; tf2::BufferCore tfBuffer; tf2_ros::TransformListener * tf_; @@ -106,7 +107,6 @@ class BaseAssembler assemble_scans_server_; message_filters::Subscriber scan_sub_; - message_filters::Connection tf_filter_connection_; //! \brief Callback function for every time we receive a new scan virtual void msgCallback(const std::shared_ptr & scan_ptr); @@ -223,6 +223,7 @@ void BaseAssembler::start(const std::string & in_topic_name) 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_, in_topic_name); tf_filter_ = new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); diff --git a/include/laser_assembler/message_filter.hpp b/include/laser_assembler/message_filter.hpp index 7ff0b17..9dbaaf9 100644 --- a/include/laser_assembler/message_filter.hpp +++ b/include/laser_assembler/message_filter.hpp @@ -291,6 +291,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi 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)); diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index e654fe8..fb5c8fb 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -22,7 +22,6 @@ #include "laser_assembler/base_assembler.hpp" #include "rclcpp/time.hpp" #include "sensor_msgs/msg/laser_scan.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #define TIME rclcpp::Time @@ -83,7 +82,7 @@ class LaserScanAssembler : public BaseAssembler unsigned int n_pts = scan_in.ranges.size(); // TODO(vandana) ignore_laser_skew_ this case is not handled right now, as there is - // no transformPointCloud support for PointCloud in ROS2. + // 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) { diff --git a/src/merge_clouds.cpp b/src/merge_clouds.cpp index 7ba3d1e..e18b585 100644 --- a/src/merge_clouds.cpp +++ b/src/merge_clouds.cpp @@ -20,7 +20,6 @@ potentially from different sensors **/ -// #include #include #include #include From 4beead3e09853492ac045273c7072cf61b034ffd Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Tue, 1 Jan 2019 18:24:17 +0530 Subject: [PATCH 25/29] - Changed "filters/filter_chain.h" include to "filters/filter_chain.hpp' because it is changed in filters package. - changed string type variable to std::string type. --- ros2_migration_readme.md | 1 - src/laser_scan_assembler.cpp | 4 ++-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ros2_migration_readme.md b/ros2_migration_readme.md index 7df8139..07f06d6 100644 --- a/ros2_migration_readme.md +++ b/ros2_migration_readme.md @@ -58,7 +58,6 @@ Build packages mkdir launch_ws/src cd launch_ws/src git clone https://github.com/ros2/launch.git -b master - git clone https://github.com/ros2/launch/tree/master Go to launch_ws directory. colcon build diff --git a/src/laser_scan_assembler.cpp b/src/laser_scan_assembler.cpp index fb5c8fb..f12f38c 100644 --- a/src/laser_scan_assembler.cpp +++ b/src/laser_scan_assembler.cpp @@ -18,7 +18,7 @@ #include #include #include "laser_geometry/laser_geometry.hpp" -#include "filters/filter_chain.h" +#include "filters/filter_chain.hpp" #include "laser_assembler/base_assembler.hpp" #include "rclcpp/time.hpp" #include "sensor_msgs/msg/laser_scan.hpp" @@ -67,7 +67,7 @@ class LaserScanAssembler : public BaseAssembler } void ConvertToCloud( - const string & fixed_frame_id, + const std::string & fixed_frame_id, const sensor_msgs::msg::LaserScan & scan_in, sensor_msgs::msg::PointCloud & cloud_out) { From ed039104d99d2f41de28eff5fbea1cfd876f4ff6 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Wed, 2 Jan 2019 14:09:44 +0530 Subject: [PATCH 26/29] Renamed ros2_migration_readme.md file to README.md. --- ros2_migration_readme.md => README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename ros2_migration_readme.md => README.md (98%) diff --git a/ros2_migration_readme.md b/README.md similarity index 98% rename from ros2_migration_readme.md rename to README.md index 07f06d6..320e901 100644 --- a/ros2_migration_readme.md +++ b/README.md @@ -1,4 +1,4 @@ -This file describes the work done and steps to perform test on the laser_assembler package. +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" From 47fc3b1528d2170a17691406eb991ba0879ceec1 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Tue, 15 Jan 2019 15:42:35 +0530 Subject: [PATCH 27/29] Changed raw node pointer to shared pointer. --- include/laser_assembler/base_assembler.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index f3f3364..89d471d 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -96,7 +96,7 @@ class BaseAssembler protected: // message_filters's subscribe method requires raw node pointer. - rclcpp::Node * n_; + rclcpp::Node::SharedPtr n_; tf2::BufferCore tfBuffer; tf2_ros::TransformListener * tf_; tf2_ros::MessageFilter * tf_filter_; @@ -143,7 +143,7 @@ BaseAssembler::BaseAssembler( { // **** Initialize TransformListener **** double tf_cache_time_secs; - n_ = node_.get(); + n_ = node_; n_->get_parameter_or("tf_cache_time_secs", tf_cache_time_secs, 10.0); @@ -224,7 +224,7 @@ void BaseAssembler::start(const std::string & in_topic_name) "leak memory"); } else { // subscribe method requires raw node pointer. - scan_sub_.subscribe(n_, in_topic_name); + scan_sub_.subscribe(n_.get(), in_topic_name); tf_filter_ = new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); tf_filter_->registerCallback( @@ -241,7 +241,7 @@ void BaseAssembler::start() RCLCPP_ERROR(g_logger, "assembler::start() was called twice!. This is bad, and could " "leak memory"); } else { - scan_sub_.subscribe(n_, "bogus"); + scan_sub_.subscribe(n_.get(), "bogus"); tf_filter_ = new tf2_ros::MessageFilter(scan_sub_, tfBuffer, fixed_frame_, 10, 0); tf_filter_->registerCallback( From 125b134eabe75c04a22006faf0c29b423707b268 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Thu, 17 Jan 2019 13:02:07 +0530 Subject: [PATCH 28/29] Removed filters_FOUND flag from CMakeLists.txt file. Updated CMakeLists.txt file from filters package has solved issue. Setting this flag is not more required. --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 82831d6..9757c0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,8 +5,6 @@ if(NOT WIN32) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") endif() -set( filters_FOUND 1 ) - find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(builtin_interfaces REQUIRED) From fcfece95e7f83a8f660dc05b2dd680120e37c6a8 Mon Sep 17 00:00:00 2001 From: vandanamandlik Date: Fri, 18 Jan 2019 14:27:12 +0530 Subject: [PATCH 29/29] Removed laser_assembler_srv_gen, now laser_assembler.srv is generated in laser_assembler package itself. Previously there was separate package to generate laser_assembler.srv --- CMakeLists.txt | 17 ++++++-- README.md | 1 - examples/periodic_snapshotter.cpp | 10 ++--- include/laser_assembler/base_assembler.hpp | 20 +++++----- .../laser_assembler/base_assembler_srv.hpp | 2 +- laser_assembler_srv_gen/CHANGELOG.rst | 0 laser_assembler_srv_gen/CMakeLists.txt | 22 ----------- laser_assembler_srv_gen/mainpage.dox | 0 laser_assembler_srv_gen/package.xml | 39 ------------------- laser_assembler_srv_gen/srv/AssembleScans.srv | 9 ----- .../srv/AssembleScans2.srv | 9 ----- package.xml | 10 ++++- srv/AssembleScans.srv | 6 +-- srv/AssembleScans2.srv | 6 +-- test/test_assembler.cpp | 10 ++--- 15 files changed, 49 insertions(+), 112 deletions(-) delete mode 100644 laser_assembler_srv_gen/CHANGELOG.rst delete mode 100644 laser_assembler_srv_gen/CMakeLists.txt delete mode 100644 laser_assembler_srv_gen/mainpage.dox delete mode 100644 laser_assembler_srv_gen/package.xml delete mode 100644 laser_assembler_srv_gen/srv/AssembleScans.srv delete mode 100644 laser_assembler_srv_gen/srv/AssembleScans2.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 9757c0d..ec35889 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,7 @@ endif() 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) @@ -15,10 +16,13 @@ find_package(message_filters REQUIRED) find_package(laser_geometry REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) -find_package(laser_assembler_srv_gen 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} @@ -26,7 +30,6 @@ set(INCLUDE_DIRS include ${ament_cmake_INCLUDE_DIRS} ${laser_geometry_INCLUDE_DIRS} ${tf2_ros_INCLUDE_DIRS} ${tf2_INCLUDE_DIRS} ${message_filters_INCLUDE_DIRS} - ${laser_assembler_srv_gen_INCLUDE_DIRS} ${rostest_INCLUDE_DIRS} ${system_INCLUDE_DIRS} ${geometry_msgs_INCLUDE_DIRS} ${filters_INCLUDE_DIRS}) @@ -35,7 +38,7 @@ include_directories(${INCLUDE_DIRS}) set(LIBS ${rclcpp_LIBRARIES} ${ament_cmake_LIBRARIES} ${rosidl_default_generators_LIBRARIES} ${signals_LIBRARIES} - ${laser_assembler_srv_gen_LIBRARIES} ${std_msgs_LIBRARIES} + ${std_msgs_LIBRARIES} ${system_LIBRARIES} ${tf2_ros_LIBRARIES} ${tf2_LIBRARIES} ${laser_geometry_LIBRARIES} ${geometry_msgs_LIBRARIES} @@ -44,6 +47,8 @@ set(LIBS ${rclcpp_LIBRARIES} ${ament_cmake_LIBRARIES} add_executable(laser_scan_assembler src/laser_scan_assembler.cpp) 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}) @@ -82,18 +87,24 @@ if(BUILD_TESTING) add_executable(dummy_scan_producer test/dummy_scan_producer.cpp) 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(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}) diff --git a/README.md b/README.md index 320e901..987ef96 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,6 @@ Build packages mkdir laser_assembler_ws mkdir laser_assembler_ws/src git clone https://github.com/vandanamandlik/laser_assembler.git -b ros2-devel - remove laser_assembler_srv_gen folder from laser_assembler and paste it in src folder (so your src folder should contain laser_assembler and laser_assembler_srv_gen directories.) Go to laser_assembler_ws source (eg. source filters_ws/install/setup.sh) source diff --git a/examples/periodic_snapshotter.cpp b/examples/periodic_snapshotter.cpp index 74bb301..5dccf2e 100644 --- a/examples/periodic_snapshotter.cpp +++ b/examples/periodic_snapshotter.cpp @@ -18,7 +18,7 @@ #include #include // Services -#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" +#include "laser_assembler/srv/assemble_scans.hpp" #include "rclcpp/rclcpp.hpp" // Messages @@ -42,7 +42,7 @@ class PeriodicSnapshotter : public rclcpp::Node : Node(service_name) { static const char SERVICE_NAME[] = "assemble_scans"; - client_ = this->create_client( + client_ = this->create_client( SERVICE_NAME); using namespace std::chrono_literals; @@ -84,14 +84,14 @@ class PeriodicSnapshotter : public rclcpp::Node // Populate our service request based on our timer callback times auto request = std::make_shared< - laser_assembler_srv_gen::srv::AssembleScans::Request>(); + 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_gen::srv::AssembleScans>::SharedFuture; + laser_assembler::srv::AssembleScans>::SharedFuture; auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get(); @@ -109,7 +109,7 @@ class PeriodicSnapshotter : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_; - rclcpp::Client::SharedPtr + rclcpp::Client::SharedPtr client_; builtin_interfaces::msg::Time last_time; rclcpp::TimerBase::SharedPtr timer_; diff --git a/include/laser_assembler/base_assembler.hpp b/include/laser_assembler/base_assembler.hpp index 89d471d..ac8149b 100644 --- a/include/laser_assembler/base_assembler.hpp +++ b/include/laser_assembler/base_assembler.hpp @@ -43,8 +43,8 @@ #include "laser_assembler/point_cloud_conversion.hpp" // Service -#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" -#include "laser_assembler_srv_gen/srv/assemble_scans2.hpp" +#include "laser_assembler/srv/assemble_scans.hpp" +#include "laser_assembler/srv/assemble_scans2.hpp" rclcpp::Logger g_logger = rclcpp::get_logger("laser_scan_assembler"); @@ -103,7 +103,7 @@ class BaseAssembler private: // ROS Input/Ouptut Handling - rclcpp::Service::SharedPtr + rclcpp::Service::SharedPtr assemble_scans_server_; message_filters::Subscriber scan_sub_; @@ -113,9 +113,9 @@ class BaseAssembler //! \brief Service Callback function called whenever we need to build a cloud bool assembleScans( - std::shared_ptr + std::shared_ptr request, - std::shared_ptr + std::shared_ptr response); //! \brief Stores history of scans @@ -196,16 +196,16 @@ BaseAssembler::BaseAssembler( // ***** Start Services ***** auto assembleScansCallback = [this]( - std::shared_ptr + std::shared_ptr request, - std::shared_ptr + std::shared_ptr response) -> bool { this->assembleScans(request, response); return true; }; assemble_scans_server_ = - n_->create_service( + n_->create_service( "assemble_scans", assembleScansCallback); // ***** Start Listening to Data ***** @@ -298,8 +298,8 @@ void BaseAssembler::msgCallback(const std::shared_ptr & scan_ptr) template bool BaseAssembler::assembleScans( - std::shared_ptr req, - std::shared_ptr + std::shared_ptr req, + std::shared_ptr resp) { scan_hist_mutex_.lock(); diff --git a/include/laser_assembler/base_assembler_srv.hpp b/include/laser_assembler/base_assembler_srv.hpp index 37e0690..5403003 100644 --- a/include/laser_assembler/base_assembler_srv.hpp +++ b/include/laser_assembler/base_assembler_srv.hpp @@ -29,7 +29,7 @@ #include "sensor_msgs/msg/point_cloud.hpp" // Service -#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" +#include "laser_assembler/srv/assemble_scans.hpp" namespace laser_assembler { diff --git a/laser_assembler_srv_gen/CHANGELOG.rst b/laser_assembler_srv_gen/CHANGELOG.rst deleted file mode 100644 index e69de29..0000000 diff --git a/laser_assembler_srv_gen/CMakeLists.txt b/laser_assembler_srv_gen/CMakeLists.txt deleted file mode 100644 index 151167f..0000000 --- a/laser_assembler_srv_gen/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(laser_assembler_srv_gen) - -# Support C++14 -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") - -if(NOT WIN32) - set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra") -endif() - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(builtin_interfaces REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(sensor_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} "srv/AssembleScans.srv" - "srv/AssembleScans2.srv" - DEPENDENCIES builtin_interfaces sensor_msgs) - -ament_export_dependencies(rosidl_default_runtime) -ament_package() diff --git a/laser_assembler_srv_gen/mainpage.dox b/laser_assembler_srv_gen/mainpage.dox deleted file mode 100644 index e69de29..0000000 diff --git a/laser_assembler_srv_gen/package.xml b/laser_assembler_srv_gen/package.xml deleted file mode 100644 index 2d38eb8..0000000 --- a/laser_assembler_srv_gen/package.xml +++ /dev/null @@ -1,39 +0,0 @@ - - - - laser_assembler_srv_gen - 1.8.4 - - Generates Assemble scans service. - - Jonathan Binney - BSD - - http://wiki.ros.org/laser_assembler - - https://github.com/ros-perception/laser_assembler.git - Vijay Pradeep - - ament_cmake - sensor_msgs - builtin_interfaces - rosidl_default_generators - - rclcpp - bondcpp - rosidl_default_runtime - sensor_msgs - - builtin_interfaces - rclcpp - - - - rosidl_interface_packages - - - ament_cmake - - - diff --git a/laser_assembler_srv_gen/srv/AssembleScans.srv b/laser_assembler_srv_gen/srv/AssembleScans.srv deleted file mode 100644 index 3c7b31b..0000000 --- a/laser_assembler_srv_gen/srv/AssembleScans.srv +++ /dev/null @@ -1,9 +0,0 @@ -# The time interval on which we want to aggregate scans -int64 begin -# The end of the interval on which we want to assemble scans or clouds -int64 end ---- -# The point cloud holding the assembled clouds or scans. -# This cloud is in the frame specified by the ~fixed_frame node parameter. -# cloud is empty if no scans are received in the requested interval. -sensor_msgs/PointCloud cloud diff --git a/laser_assembler_srv_gen/srv/AssembleScans2.srv b/laser_assembler_srv_gen/srv/AssembleScans2.srv deleted file mode 100644 index e2bafd1..0000000 --- a/laser_assembler_srv_gen/srv/AssembleScans2.srv +++ /dev/null @@ -1,9 +0,0 @@ -# The time interval on which we want to aggregate scans -int64 begin -# The end of the interval on which we want to assemble scans or clouds -int64 end ---- -# The point cloud holding the assembled clouds or scans. -# This cloud is in the frame specified by the ~fixed_frame node parameter. -# cloud is empty if no scans are received in the requested interval. -sensor_msgs/PointCloud2 cloud diff --git a/package.xml b/package.xml index 8d1e4ba..a1a19f0 100644 --- a/package.xml +++ b/package.xml @@ -14,6 +14,8 @@ BSD http://ros.org/wiki/laser_assembler ament_cmake + sensor_msgs + builtin_interfaces rosidl_default_generators sensor_msgs message_filters @@ -24,7 +26,6 @@ rostest message_filters laser_geometry - laser_assembler_srv_gen builtin_interfaces sensor_msgs @@ -36,10 +37,15 @@ filters laser_geometry builtin_interfaces - laser_assembler_srv_gen + rosidl_default_runtime + sensor_msgs ament_lint_auto ament_lint_common + builtin_interfaces + + rosidl_interface_packages + ament_cmake diff --git a/srv/AssembleScans.srv b/srv/AssembleScans.srv index 083e742..3c7b31b 100644 --- a/srv/AssembleScans.srv +++ b/srv/AssembleScans.srv @@ -1,9 +1,9 @@ # The time interval on which we want to aggregate scans -uint64 begin +int64 begin # The end of the interval on which we want to assemble scans or clouds -uint64 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. # cloud is empty if no scans are received in the requested interval. -sensor_msgs/msg/PointCloud cloud +sensor_msgs/PointCloud cloud diff --git a/srv/AssembleScans2.srv b/srv/AssembleScans2.srv index fcfd11f..e2bafd1 100644 --- a/srv/AssembleScans2.srv +++ b/srv/AssembleScans2.srv @@ -1,9 +1,9 @@ # The time interval on which we want to aggregate scans -uint64 begin +int64 begin # The end of the interval on which we want to assemble scans or clouds -uint64 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. # cloud is empty if no scans are received in the requested interval. -sensor_msgs/msg/PointCloud2 cloud +sensor_msgs/PointCloud2 cloud diff --git a/test/test_assembler.cpp b/test/test_assembler.cpp index cd9eaa8..d0d959e 100644 --- a/test/test_assembler.cpp +++ b/test/test_assembler.cpp @@ -18,7 +18,7 @@ #include #include #include -#include "laser_assembler_srv_gen/srv/assemble_scans.hpp" +#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" @@ -30,7 +30,7 @@ class TestAssembler : public testing::Test { public: rclcpp::Node::SharedPtr node_; - rclcpp::Client::SharedPtr + rclcpp::Client::SharedPtr client_; TestAssembler() {} @@ -42,7 +42,7 @@ class TestAssembler : public testing::Test node_ = rclcpp::Node::make_shared("test_assembler"); std::thread spin_thread(spinThread, node_); spin_thread.detach(); - client_ = node_->create_client( + client_ = node_->create_client( "assemble_scans"); using namespace std::chrono_literals; RCLCPP_INFO(node_->get_logger(), "Waiting for service [%s]", SERVICE_NAME); @@ -73,7 +73,7 @@ class TestAssembler : public testing::Test } else { // Make the call to get a point cloud auto request = std::make_shared< - laser_assembler_srv_gen::srv::AssembleScans::Request>(); + laser_assembler::srv::AssembleScans::Request>(); request->begin = start_time_.sec; request->end = scan_msg->header.stamp.sec; @@ -89,7 +89,7 @@ class TestAssembler : public testing::Test // function can continue and our callback will get called once the // response is received. using ServiceResponseFuture = rclcpp::Client< - laser_assembler_srv_gen::srv::AssembleScans>::SharedFuture; + laser_assembler::srv::AssembleScans>::SharedFuture; auto response_received_callback = [this](ServiceResponseFuture future) { auto result = future.get();