From 035fd76ff11e73be2b7fc8134bc3c52ccd197823 Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 15 Apr 2024 20:00:25 -0400 Subject: [PATCH 01/54] setup --- CMakeLists.txt | 195 ++++-------- include/robot_body_filter/RobotBodyFilter.h | 288 +++++++++++------- .../robot_body_filter/utils/time_utils.hpp | 13 +- package.xml | 29 +- src/utils/cloud.cpp | 79 +++-- 5 files changed, 298 insertions(+), 306 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1c2ff25..d9024cb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,147 +1,76 @@ -cmake_minimum_required(VERSION 3.7.2) +# ############################################################################### +# Set minimum required version of cmake, project name and compile options +# ############################################################################### +cmake_minimum_required(VERSION 3.5) project(robot_body_filter) -set (CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${CMAKE_CURRENT_SOURCE_DIR}/cmake") -include(DetectOptional) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -if(${ROBOT_BODY_FILTER_HAVE_CXX_OPTIONAL}) - # add_compile_definitions would be nicer, but isn't available in Stretch - add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=1) -else() - add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(THIS_PACKAGE_DEPS dynamic_reconfigure filters geometric_shapes laser_geometry moveit_core moveit_ros_perception roscpp sensor_msgs tf2 tf2_ros urdf visualization_msgs) -set(MESSAGE_DEPS geometry_msgs std_msgs) +# Suppress all warnings +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") + +# ############################################################################### +# Find ament packages and libraries for ament and system dependencies +# ############################################################################### + +# find dependencies +set(THIS_PACKAGE_INCLUDE_DEPENDS + + # dynamic_reconfigure # may be superceeded by using ros2 parameters + filters + geometric_shapes + geometry_msgs + laser_geometry + + # instead of libpcl-all-dev? + pcl_ros + moveit_core + + # moveit_ros_perception + + # roscpp replaced by rclcpp + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros + urdf + visualization_msgs + pcl_conversions + fcl + tf2_eigen + tf2_sensor_msgs +) -find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_DEPS} ${MESSAGE_DEPS} message_generation pcl_conversions tf2_eigen tf2_sensor_msgs) -find_package(PCL REQUIRED COMPONENTS common filters) -find_package(Threads REQUIRED) +# find dependencies +find_package(ament_cmake REQUIRED) -find_package(PkgConfig REQUIRED) -pkg_check_modules(LIBFCL_PC REQUIRED fcl) -# find *absolute* paths to LIBFCL_* paths -find_path(LIBFCL_INCLUDE_DIRS fcl/config.h HINTS ${LIBFCL_PC_INCLUDE_DIR} ${LIBFCL_PC_INCLUDE_DIRS}) -set(LIBFCL_LIBRARIES) -foreach(_lib ${LIBFCL_PC_LIBRARIES}) - find_library(_lib_${_lib} ${_lib} HINTS ${LIBFCL_PC_LIBRARY_DIRS}) - list(APPEND LIBFCL_LIBRARIES ${_lib_${_lib}}) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) endforeach() - -add_message_files(FILES OrientedBoundingBox.msg OrientedBoundingBoxStamped.msg Sphere.msg SphereStamped.msg) -generate_messages(DEPENDENCIES ${MESSAGE_DEPS}) - -catkin_package( - CATKIN_DEPENDS ${THIS_PACKAGE_DEPS} ${MESSAGE_DEPS} message_runtime - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} RayCastingShapeMask TFFramesWatchdog ${PROJECT_NAME}_utils +# build +include_directories( + include + include/utils + ${Dependency}_INCLUDE_DIRS ) -include_directories(SYSTEM ${LIBFCL_INCLUDE_DIRS}) -include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) - -set(UTILS_SRCS - src/utils/bodies.cpp - src/utils/cloud.cpp - src/utils/shapes.cpp - src/utils/string_utils.cpp - src/utils/tf2_eigen.cpp - src/utils/tf2_sensor_msgs.cpp - src/utils/time_utils.cpp) - -if (PCL_VERSION VERSION_LESS "1.10") - set(UTILS_SRCS ${UTILS_SRCS} src/utils/crop_box.cpp) -endif() - -add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) -target_link_libraries(${PROJECT_NAME}_utils ${catkin_LIBRARIES} ${LIBFCL_LIBRARIES} ${PCL_LIBRARIES}) +add_executable(${PROJECT_NAME}_filter src/RobotBodyFilter.cpp) +ament_target_dependencies(${PROJECT_NAME}_filter ${THIS_PACKAGE_INCLUDE_DEPENDS}) -add_library(tf2_sensor_msgs_rbf src/utils/tf2_sensor_msgs.cpp) -target_link_libraries(tf2_sensor_msgs_rbf PRIVATE ${PROJECT_NAME}_utils PUBLIC ${catkin_LIBRARIES}) -add_dependencies(tf2_sensor_msgs_rbf ${catkin_EXPORTED_TARGETS}) - -add_library(RayCastingShapeMask src/RayCastingShapeMask.cpp) -target_link_libraries(RayCastingShapeMask ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) -add_dependencies(RayCastingShapeMask ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -add_library(TFFramesWatchdog src/TFFramesWatchdog.cpp) -target_link_libraries(TFFramesWatchdog ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) -add_dependencies(TFFramesWatchdog ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) -target_link_libraries(${PROJECT_NAME} - ${PROJECT_NAME}_utils - tf2_sensor_msgs_rbf - TFFramesWatchdog - RayCastingShapeMask - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} +target_include_directories(${PROJECT_NAME}_filter + PUBLIC + $ + $ + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask ${PROJECT_NAME}_utils tf2_sensor_msgs_rbf - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(FILES laser_filters.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/) - -install(FILES rviz/debug.rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz) - -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - - catkin_add_gtest(test_set_utils test/test_set_utils.cpp) - target_link_libraries(test_set_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_shapes_rbf test/test_shapes.cpp) - target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_urdf_eigen test/test_urdf_eigen.cpp) - target_link_libraries(test_urdf_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_time_utils test/test_time_utils.cpp) - target_link_libraries(test_time_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_tf2_eigen test/test_tf2_eigen.cpp) - target_link_libraries(test_tf2_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_string_utils test/test_string_utils.cpp) - target_link_libraries(test_string_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_bodies test/test_bodies.cpp) - target_link_libraries(test_bodies ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_cloud test/test_cloud.cpp) - target_link_libraries(test_cloud ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_tf2_sensor_msgs test/test_tf2_sensor_msgs.cpp) - target_link_libraries(test_tf2_sensor_msgs tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - catkin_add_gtest(test_xmlrpc_traits test/test_xmlrpc_traits.cpp) - target_link_libraries(test_xmlrpc_traits ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - - add_rostest_gtest(test_filter_utils test/test_filter_utils.test test/test_filter_utils.cpp) - target_link_libraries(test_filter_utils ${PROJECT_NAME} ${catkin_LIBRARIES}) - - catkin_add_gtest(test_tf_frames_watchdog test/test_tf_frames_watchdog.cpp) - target_link_libraries(test_tf_frames_watchdog TFFramesWatchdog ${catkin_LIBRARIES}) - - catkin_add_gtest(test_ray_casting_shape_mask test/test_ray_casting_shape_mask.cpp) - target_link_libraries(test_ray_casting_shape_mask RayCastingShapeMask ${catkin_LIBRARIES}) - if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") - target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=1) - else() - target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=0) - endif() - add_rostest_gtest(test_robot_body_filter test/test_robot_body_filter.test test/test_robot_body_filter.cpp) - target_link_libraries(test_robot_body_filter ${PROJECT_NAME} ${catkin_LIBRARIES}) - target_compile_options(test_robot_body_filter PRIVATE -fno-var-tracking-assignments) # speed up build -endif() \ No newline at end of file +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() \ No newline at end of file diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 203d0d8..c10a157 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -12,34 +12,39 @@ #include -#include +// #include +// #include +#include +// #include +// #include +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include #include -#include -#include -#include -#include +#include #include -#include #include namespace robot_body_filter { /** -* \brief Just a helper structure holding together a link, one of its collision elements, - * and the index of the collision element in the collision array of the link. -*/ + * \brief Just a helper structure holding together a link, one of its collision + * elements, and the index of the collision element in the collision array of + * the link. + */ struct CollisionBodyWithLink { urdf::CollisionSharedPtr collision; urdf::LinkSharedPtr link; @@ -47,40 +52,40 @@ struct CollisionBodyWithLink { MultiShapeHandle multiHandle; std::string cacheKey; - CollisionBodyWithLink() : - indexInCollisionArray(0), cacheKey("__empty__") { - } + CollisionBodyWithLink() : indexInCollisionArray(0), cacheKey("__empty__") {} CollisionBodyWithLink(urdf::CollisionSharedPtr collision, urdf::LinkSharedPtr link, const size_t indexInCollisionArray, - const MultiShapeHandle& multiHandle): - collision(collision), link(link), indexInCollisionArray(indexInCollisionArray), - multiHandle(multiHandle) - { + const MultiShapeHandle &multiHandle) + : collision(collision), link(link), + indexInCollisionArray(indexInCollisionArray), multiHandle(multiHandle) { std::ostringstream stream; stream << link->name << "-" << indexInCollisionArray; this->cacheKey = stream.str(); } }; -struct ScaleAndPadding -{ +struct ScaleAndPadding { double scale; double padding; ScaleAndPadding(double scale = 1.0, double padding = 0.0); - bool operator==(const ScaleAndPadding& other) const; - bool operator!=(const ScaleAndPadding& other) const; + bool operator==(const ScaleAndPadding &other) const; + bool operator!=(const ScaleAndPadding &other) const; }; -/** \brief Suffix added to link/collision names to distinguish their usage in contains tests only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in + * contains tests only. */ static const std::string CONTAINS_SUFFIX = "::contains"; -/** \brief Suffix added to link/collision names to distinguish their usage in shadow tests only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in + * shadow tests only. */ static const std::string SHADOW_SUFFIX = "::shadow"; -/** \brief Suffix added to link/collision names to distinguish their usage in bounding sphere computation only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in + * bounding sphere computation only. */ static const std::string BSPHERE_SUFFIX = "::bounding_sphere"; -/** \brief Suffix added to link/collision names to distinguish their usage in bounding box computation only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in + * bounding box computation only. */ static const std::string BBOX_SUFFIX = "::bounding_box"; /** @@ -90,7 +95,7 @@ static const std::string BBOX_SUFFIX = "::bounding_box"; * * \author Martin Pecka */ -template +template class RobotBodyFilter : public ::robot_body_filter::FilterBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -102,10 +107,9 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { //! Parameters are described in the readme. bool configure() override; - bool update(const T& data_in, T& data_out) override = 0; + bool update(const T &data_in, T &data_out) override = 0; protected: - //! Handle of the node this filter runs in. ros::NodeHandle nodeHandle; ros::NodeHandle privateNodeHandle; @@ -155,45 +159,56 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { //! The minimum distance of points from the sensor to keep them (in meters). double minDistance; - //! The maximum distance of points from the sensor origin to apply this filter on (in meters). + //! The maximum distance of points from the sensor origin to apply this filter + //! on (in meters). double maxDistance; - //! The default inflation that is applied to the collision model for the purposes of checking if a point is contained - //! by the robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Every collision element is scaled - //! individually with the scaling center in its origin. Padding is added individually to every collision element. + //! The default inflation that is applied to the collision model for the + //! purposes of checking if a point is contained by the robot model (scale 1.0 + //! = no scaling, padding 0.0 = no padding). Every collision element is scaled + //! individually with the scaling center in its origin. Padding is added + //! individually to every collision element. ScaleAndPadding defaultContainsInflation; - //! The default inflation that is applied to the collision model for the purposes of checking if a point is shadowed - //! by the robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Every collision element is scaled - //! individually with the scaling center in its origin. Padding is added individually to every collision element. + //! The default inflation that is applied to the collision model for the + //! purposes of checking if a point is shadowed by the robot model (scale 1.0 + //! = no scaling, padding 0.0 = no padding). Every collision element is scaled + //! individually with the scaling center in its origin. Padding is added + //! individually to every collision element. ScaleAndPadding defaultShadowInflation; - //! The default inflation that is applied to the collision model for the purposes of computing the bounding sphere. - //! Every collision element is scaled individually with the scaling center in its origin. Padding is added - //! individually to every collision element. + //! The default inflation that is applied to the collision model for the + //! purposes of computing the bounding sphere. Every collision element is + //! scaled individually with the scaling center in its origin. Padding is + //! added individually to every collision element. ScaleAndPadding defaultBsphereInflation; - //! The default inflation that is applied to the collision model for the purposes of computing the bounding box. - //! Every collision element is scaled individually with the scaling center in its origin. Padding is added + //! The default inflation that is applied to the collision model for the + //! purposes of computing the bounding box. Every collision element is scaled + //! individually with the scaling center in its origin. Padding is added //! individually to every collision element. ScaleAndPadding defaultBboxInflation; - //! Inflation that is applied to a collision element for the purposes of checking if a point is contained by the - //! robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Elements not present in this list are scaled and - //! padded with defaultContainsInflation. + //! Inflation that is applied to a collision element for the purposes of + //! checking if a point is contained by the robot model (scale 1.0 = no + //! scaling, padding 0.0 = no padding). Elements not present in this list are + //! scaled and padded with defaultContainsInflation. std::map perLinkContainsInflation; - //! Inflation that is applied to a collision element for the purposes of checking if a point is shadowed by the - //! robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Elements not present in this list are scaled and - //! padded with defaultShadowInflation. + //! Inflation that is applied to a collision element for the purposes of + //! checking if a point is shadowed by the robot model (scale 1.0 = no + //! scaling, padding 0.0 = no padding). Elements not present in this list are + //! scaled and padded with defaultShadowInflation. std::map perLinkShadowInflation; - //! Inflation that is applied to a collision element for the purposes of computing the bounding sphere. - //! Elements not present in this list are scaled and padded with defaultBsphereInflation. + //! Inflation that is applied to a collision element for the purposes of + //! computing the bounding sphere. Elements not present in this list are + //! scaled and padded with defaultBsphereInflation. std::map perLinkBsphereInflation; - //! Inflation that is applied to a collision element for the purposes of computing the bounding box. - //! Elements not present in this list are scaled and padded with defaultBboxInflation. + //! Inflation that is applied to a collision element for the purposes of + //! computing the bounding box. Elements not present in this list are scaled + //! and padded with defaultBboxInflation. std::map perLinkBboxInflation; //! Name of the parameter where the robot model can be found. @@ -201,7 +216,8 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { //! Subscriber for robot_description updates. ros::Subscriber robotDescriptionUpdatesListener; - //! Name of the field in the dynamic reconfigure message that contains robot model. + //! Name of the field in the dynamic reconfigure message that contains robot + //! model. std::string robotDescriptionUpdatesFieldName; std::set linksIgnoredInBoundingSphere; @@ -281,7 +297,8 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { bool publishBoundingSphereMarker; //! Whether to publish scan_point_cloud with robot bounding box cut out. bool publishNoBoundingBoxPointcloud; - //! Whether to publish scan_point_cloud with robot oriented bounding box cut out. + //! Whether to publish scan_point_cloud with robot oriented bounding box cut + //! out. bool publishNoOrientedBoundingBoxPointcloud; //! Whether to publish scan_point_cloud with robot local bounding box cut out. bool publishNoLocalBoundingBoxPointcloud; @@ -307,7 +324,8 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { //! Whether to process data when there are some unreachable frames. bool requireAllFramesReachable; - //! A mutex that has to be locked in order to work with shapesToLinks or tfBuffer. + //! A mutex that has to be locked in order to work with shapesToLinks or + //! tfBuffer. std::shared_ptr modelMutex; //! tf buffer length @@ -326,23 +344,31 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { //! Tool for masking out 3D bodies out of point clouds. std::unique_ptr shapeMask; - /// A map that correlates shapes in robot_shape_mask to collision links in URDF. + /// A map that correlates shapes in robot_shape_mask to collision links in + /// URDF. /** Keys are shape handles from shapeMask. */ - std::map shapesToLinks; + std::map + shapesToLinks; std::set shapesIgnoredInBoundingSphere; std::set shapesIgnoredInBoundingBox; - //! Caches any link->fixedFrame transforms after a scan message is received. Is queried by robot_shape_mask. Keys are CollisionBodyWithLink#cacheKey. - std::map > transformCache; - //! Caches any link->fixedFrame transforms at the time of scan end. Only used for pointByPoint scans. Is queried by robot_shape_mask. Keys are CollisionBodyWithLink#cacheKey. - std::map > transformCacheAfterScan; - - //! If the scan is pointByPoint, set this variable to the ratio between scan start and end time you're looking for with getShapeTransform(). + //! Caches any link->fixedFrame transforms after a scan message is received. + //! Is queried by robot_shape_mask. Keys are CollisionBodyWithLink#cacheKey. + std::map> transformCache; + //! Caches any link->fixedFrame transforms at the time of scan end. Only used + //! for pointByPoint scans. Is queried by robot_shape_mask. Keys are + //! CollisionBodyWithLink#cacheKey. + std::map> + transformCacheAfterScan; + + //! If the scan is pointByPoint, set this variable to the ratio between scan + //! start and end time you're looking for with getShapeTransform(). mutable double cacheLookupBetweenScansRatio; - //! Used in tests. If false, configure() waits until robot description becomes available. If true, - //! configure() fails with std::runtime_exception if robot description is not available. + //! Used in tests. If false, configure() waits until robot description becomes + //! available. If true, configure() fails with std::runtime_exception if robot + //! description is not available. bool failWithoutRobotDescription = false; /** @@ -359,28 +385,32 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { * sensor position from the viewpoint channels. * \return Whether the computation succeeded. */ - bool computeMask(const sensor_msgs::PointCloud2& projectedPointCloud, - std::vector& mask, - const std::string& sensorFrame = ""); + bool computeMask(const sensor_msgs::PointCloud2 &projectedPointCloud, + std::vector &mask, + const std::string &sensorFrame = ""); - /** \brief Return the latest cached transform for the link corresponding to the given shape handle. + /** \brief Return the latest cached transform for the link corresponding to + * the given shape handle. * * You should call updateTransformCache before calling this function. * - * \param shapeHandle The handle of the shape for which we want the transform. The handle is from robot_shape_mask. - * \param[out] transform Transform of the corresponding link (wrt filtering frame). - * \return If the transform was found. + * \param shapeHandle The handle of the shape for which we want the transform. + * The handle is from robot_shape_mask. \param[out] transform Transform of the + * corresponding link (wrt filtering frame). \return If the transform was + * found. */ - bool getShapeTransform(point_containment_filter::ShapeHandle shapeHandle, Eigen::Isometry3d& transform) const; + bool getShapeTransform(point_containment_filter::ShapeHandle shapeHandle, + Eigen::Isometry3d &transform) const; /** \brief Update robot_shape_mask with the given URDF model. * * \param urdfModel The robot's URDF loaded as a string. */ - void addRobotMaskFromUrdf(const std::string& urdfModel); + void addRobotMaskFromUrdf(const std::string &urdfModel); /** - * \brief Remove all parts of the robot mask and clear internal shape and TF buffers. + * \brief Remove all parts of the robot mask and clear internal shape and TF + * buffers. * * Make sure no filtering happens when executing this function. */ @@ -389,12 +419,15 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { /** \brief Update the cache of link transforms relative to filtering frame. * * \param time The time to get transforms for. - * \param afterScantime The after scan time to get transforms for (if zero time is passed, after scan transforms are not computed). + * \param afterScantime The after scan time to get transforms for (if zero + * time is passed, after scan transforms are not computed). */ - void updateTransformCache(const ros::Time& time, const ros::Time& afterScanTime = ros::Time(0)); + void updateTransformCache(const ros::Time &time, + const ros::Time &afterScanTime = ros::Time(0)); /** - * \brief Callback handling update of the robot_description parameter using dynamic reconfigure. + * \brief Callback handling update of the robot_description parameter using + * dynamic reconfigure. * * \param newConfig The updated config. */ @@ -404,74 +437,95 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { * \brief Callback for ~reload_model service. Reloads the URDF from parameter. * \return Success. */ - bool triggerModelReload(std_srvs::TriggerRequest&, std_srvs::TriggerResponse&); + bool triggerModelReload(std_srvs::TriggerRequest &, + std_srvs::TriggerResponse &); void createBodyVisualizationMsg( - const std::map& bodies, - const ros::Time& stamp, const std_msgs::ColorRGBA& color, - visualization_msgs::MarkerArray& markerArray) const; + const std::map &bodies, + const ros::Time &stamp, const std_msgs::ColorRGBA &color, + visualization_msgs::MarkerArray &markerArray) const; - void publishDebugMarkers(const ros::Time& scanTime) const; + void publishDebugMarkers(const ros::Time &scanTime) const; void publishDebugPointClouds( - const sensor_msgs::PointCloud2& projectedPointCloud, + const sensor_msgs::PointCloud2 &projectedPointCloud, const std::vector &pointMask) const; /** * \brief Computation of the bounding sphere, debug spheres, and publishing of * pointcloud without bounding sphere. */ - void computeAndPublishBoundingSphere(const sensor_msgs::PointCloud2& projectedPointCloud) const; + void computeAndPublishBoundingSphere( + const sensor_msgs::PointCloud2 &projectedPointCloud) const; /** * \brief Computation of the bounding box, debug boxes, and publishing of * pointcloud without bounding box. */ - void computeAndPublishBoundingBox(const sensor_msgs::PointCloud2& projectedPointCloud) const; + void computeAndPublishBoundingBox( + const sensor_msgs::PointCloud2 &projectedPointCloud) const; /** - * \brief Computation of the oriented bounding box, debug boxes, and publishing of - * pointcloud without bounding box. + * \brief Computation of the oriented bounding box, debug boxes, and + * publishing of pointcloud without bounding box. */ - void computeAndPublishOrientedBoundingBox(const sensor_msgs::PointCloud2& projectedPointCloud) const; + void computeAndPublishOrientedBoundingBox( + const sensor_msgs::PointCloud2 &projectedPointCloud) const; /** - * \brief Computation of the local bounding box, debug boxes, and publishing of - * pointcloud without bounding box. + * \brief Computation of the local bounding box, debug boxes, and publishing + * of pointcloud without bounding box. */ - void computeAndPublishLocalBoundingBox(const sensor_msgs::PointCloud2& projectedPointCloud) const; - - ScaleAndPadding getLinkInflationForContainsTest(const std::string& linkName) const; - ScaleAndPadding getLinkInflationForContainsTest(const std::vector& linkNames) const; - ScaleAndPadding getLinkInflationForShadowTest(const std::string& linkName) const; - ScaleAndPadding getLinkInflationForShadowTest(const std::vector& linkNames) const; - ScaleAndPadding getLinkInflationForBoundingSphere(const std::string& linkName) const; - ScaleAndPadding getLinkInflationForBoundingSphere(const std::vector& linkNames) const; - ScaleAndPadding getLinkInflationForBoundingBox(const std::string& linkName) const; - ScaleAndPadding getLinkInflationForBoundingBox(const std::vector& linkNames) const; + void computeAndPublishLocalBoundingBox( + const sensor_msgs::PointCloud2 &projectedPointCloud) const; + + ScaleAndPadding + getLinkInflationForContainsTest(const std::string &linkName) const; + ScaleAndPadding getLinkInflationForContainsTest( + const std::vector &linkNames) const; + ScaleAndPadding + getLinkInflationForShadowTest(const std::string &linkName) const; + ScaleAndPadding getLinkInflationForShadowTest( + const std::vector &linkNames) const; + ScaleAndPadding + getLinkInflationForBoundingSphere(const std::string &linkName) const; + ScaleAndPadding getLinkInflationForBoundingSphere( + const std::vector &linkNames) const; + ScaleAndPadding + getLinkInflationForBoundingBox(const std::string &linkName) const; + ScaleAndPadding getLinkInflationForBoundingBox( + const std::vector &linkNames) const; private: - ScaleAndPadding getLinkInflation(const std::vector& linkNames, const ScaleAndPadding& defaultInflation, const std::map& perLinkInflation) const; + ScaleAndPadding getLinkInflation( + const std::vector &linkNames, + const ScaleAndPadding &defaultInflation, + const std::map &perLinkInflation) const; }; -class RobotBodyFilterLaserScan : public RobotBodyFilter -{ +class RobotBodyFilterLaserScan + : public RobotBodyFilter { public: //! Apply the filter. - bool update(const sensor_msgs::LaserScan &inputScan, sensor_msgs::LaserScan &filteredScan) override; + bool update(const sensor_msgs::LaserScan &inputScan, + sensor_msgs::LaserScan &filteredScan) override; bool configure() override; protected: laser_geometry::LaserProjection laserProjector; - // in RobotBodyFilterLaserScan::update we project the scan to a pointcloud with viewpoints - const std::unordered_map channelsToTransform { {"vp_", CloudChannelType::POINT} }; + // in RobotBodyFilterLaserScan::update we project the scan to a pointcloud + // with viewpoints + const std::unordered_map channelsToTransform{ + {"vp_", CloudChannelType::POINT}}; }; -class RobotBodyFilterPointCloud2 : public RobotBodyFilter -{ +class RobotBodyFilterPointCloud2 + : public RobotBodyFilter { public: //! Apply the filter. - bool update(const sensor_msgs::PointCloud2 &inputCloud, sensor_msgs::PointCloud2 &filteredCloud) override; + bool update(const sensor_msgs::PointCloud2 &inputCloud, + sensor_msgs::PointCloud2 &filteredCloud) override; bool configure() override; @@ -482,6 +536,6 @@ class RobotBodyFilterPointCloud2 : public RobotBodyFilter channelsToTransform; }; -} +} // namespace robot_body_filter -#endif //ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ +#endif // ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ diff --git a/include/robot_body_filter/utils/time_utils.hpp b/include/robot_body_filter/utils/time_utils.hpp index b75e083..ff6f748 100644 --- a/include/robot_body_filter/utils/time_utils.hpp +++ b/include/robot_body_filter/utils/time_utils.hpp @@ -1,7 +1,8 @@ #ifndef ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP #define ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP -#include +// #include +#include namespace robot_body_filter { @@ -9,7 +10,8 @@ namespace robot_body_filter { * @brief remainingTime Return remaining time to timeout from the query time. * @param query The query time, e.g. of the tf transform. * @param timeout Maximum time to wait from the query time onwards. - * @return The remaining time, or zero duration if the time is negative or ROS time isn't initialized. + * @return The remaining time, or zero duration if the time is negative or ROS + * time isn't initialized. */ ros::Duration remainingTime(const ros::Time &query, double timeout); @@ -17,11 +19,12 @@ ros::Duration remainingTime(const ros::Time &query, double timeout); * @brief remainingTime Return remaining time to timeout from the query time. * @param query The query time, e.g. of the tf transform. * @param timeout Maximum time to wait from the query time onwards. - * @return The remaining time, or zero duration if the time is negative or ROS time isn't initialized. + * @return The remaining time, or zero duration if the time is negative or ROS + * time isn't initialized. */ ros::Duration remainingTime(const ros::Time &query, const ros::Duration &timeout); -}; +}; // namespace robot_body_filter -#endif //ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP \ No newline at end of file +#endif // ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP \ No newline at end of file diff --git a/package.xml b/package.xml index 7d58a4a..3f55a8e 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ robot_body_filter - 1.3.2 + 1.3.1 Filters the robot's body out of laser scans or point clouds. @@ -14,16 +14,14 @@ Martin Pecka - catkin + ament_cmake_auto dynamic_reconfigure - filters - filters - geometric_shapes - geometric_shapes + filters + geometric_shapes geometry_msgs laser_geometry - libpcl-all-dev + libpcl-dev moveit_core moveit_ros_perception roscpp @@ -33,26 +31,19 @@ tf2_ros urdf visualization_msgs + pcl_conversions + fcl - message_generation - message_runtime - message_runtime - libfcl-dev - libfcl-dev - fcl - fcl - pcl_conversions - pcl_conversions - pkg-config tf2_eigen tf2_sensor_msgs - rostest + rosidl_default_runtime + rosidl_interface_packages - + ament_cmake diff --git a/src/utils/cloud.cpp b/src/utils/cloud.cpp index f577da0..51f0380 100644 --- a/src/utils/cloud.cpp +++ b/src/utils/cloud.cpp @@ -1,15 +1,14 @@ #include #define private protected #include +#include #undef private #include -namespace robot_body_filter -{ +namespace robot_body_filter { -size_t num_points(const Cloud &cloud) -{ +size_t num_points(const Cloud &cloud) { return size_t(cloud.height) * cloud.width; } @@ -21,45 +20,52 @@ bool hasField(const Cloud &cloud, const std::string &fieldName) { return false; } -sensor_msgs::PointField& getField(Cloud& cloud, const std::string& fieldName) { +sensor_msgs::PointField &getField(Cloud &cloud, const std::string &fieldName) { for (auto &field : cloud.fields) { if (field.name == fieldName) return field; } - throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); + throw std::runtime_error(std::string("Field ") + fieldName + + " does not exist."); } -const sensor_msgs::PointField& getField(const Cloud& cloud, const std::string& fieldName) { +const sensor_msgs::PointField &getField(const Cloud &cloud, + const std::string &fieldName) { for (const auto &field : cloud.fields) { if (field.name == fieldName) return field; } - throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); + throw std::runtime_error(std::string("Field ") + fieldName + + " does not exist."); } -size_t sizeOfPointField(const sensor_msgs::PointField& field) -{ +size_t sizeOfPointField(const sensor_msgs::PointField &field) { return sizeOfPointField(field.datatype); } -size_t sizeOfPointField(int datatype) -{ - if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8)) +size_t sizeOfPointField(int datatype) { + if ((datatype == sensor_msgs::PointField::INT8) || + (datatype == sensor_msgs::PointField::UINT8)) return 1u; - else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16)) + else if ((datatype == sensor_msgs::PointField::INT16) || + (datatype == sensor_msgs::PointField::UINT16)) return 2u; - else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) || + else if ((datatype == sensor_msgs::PointField::INT32) || + (datatype == sensor_msgs::PointField::UINT32) || (datatype == sensor_msgs::PointField::FLOAT32)) return 4u; else if (datatype == sensor_msgs::PointField::FLOAT64) return 8u; else - throw std::runtime_error(std::string("PointField of type ") + std::to_string(datatype) + " does not exist"); + throw std::runtime_error(std::string("PointField of type ") + + std::to_string(datatype) + " does not exist"); } -void copyChannelData(const Cloud& in, Cloud& out, const std::string& fieldName) { +void copyChannelData(const Cloud &in, Cloud &out, + const std::string &fieldName) { if (num_points(out) < num_points(in)) - throw std::runtime_error("Output cloud needs to be resized to fit the number of points of the input cloud."); + throw std::runtime_error("Output cloud needs to be resized to fit the " + "number of points of the input cloud."); GenericCloudConstIter dataIn(in, fieldName); GenericCloudIter dataOut(out, fieldName); @@ -67,35 +73,44 @@ void copyChannelData(const Cloud& in, Cloud& out, const std::string& fieldName) dataOut.copyData(dataIn); } - namespace impl { -template class V> -GenericCloudIteratorBase::GenericCloudIteratorBase(C& cloudMsg, const std::string& fieldName) - : sensor_msgs::impl::PointCloud2IteratorBase(cloudMsg, fieldName) { +template class V> +GenericCloudIteratorBase::GenericCloudIteratorBase( + C &cloudMsg, const std::string &fieldName) + : sensor_msgs::impl::PointCloud2IteratorBase(cloudMsg, + fieldName) { this->fieldSize = sizeOfPointField(getField(cloudMsg, fieldName)); } -template class V> -U* GenericCloudIteratorBase::getData() const { +template class V> +U *GenericCloudIteratorBase::getData() const { return this->data_; } -template -void GenericCloudIterator::copyData(const GenericCloudConstIterator& otherIter) const { +template +void GenericCloudIterator::copyData( + const GenericCloudConstIterator &otherIter) const { memcpy(this->getData(), otherIter.getData(), this->fieldSize); } -template -void GenericCloudIterator::copyData(const GenericCloudIterator& otherIter) const { +template +void GenericCloudIterator::copyData( + const GenericCloudIterator &otherIter) const { memcpy(this->getData(), otherIter.getData(), this->fieldSize); } // explicitly instantiate -template class GenericCloudIteratorBase; -template class GenericCloudIteratorBase; +template class GenericCloudIteratorBase; +template class GenericCloudIteratorBase< + unsigned char, const unsigned char, const unsigned char, + const sensor_msgs::PointCloud2, GenericCloudConstIterator>; template class GenericCloudIterator; template class GenericCloudConstIterator; -} +} // namespace impl -} +} // namespace robot_body_filter From c9429922fb45f23a1007fc8d2b3f960273f08f61 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Tue, 16 Apr 2024 09:26:14 -0400 Subject: [PATCH 02/54] Fixed headers --- include/robot_body_filter/RobotBodyFilter.h | 4 ++-- package.xml | 13 +++++-------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index c10a157..936490f 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -21,11 +21,11 @@ #include #include #include -#include +#include #include #include #include -#include +// #include #include #include #include diff --git a/package.xml b/package.xml index 3f55a8e..425fc73 100644 --- a/package.xml +++ b/package.xml @@ -16,15 +16,15 @@ ament_cmake_auto - dynamic_reconfigure + filters geometric_shapes geometry_msgs laser_geometry - libpcl-dev + moveit_core moveit_ros_perception - roscpp + rclcpp sensor_msgs std_msgs tf2 @@ -32,10 +32,7 @@ urdf visualization_msgs pcl_conversions - fcl - - - + tf2_eigen tf2_sensor_msgs @@ -46,4 +43,4 @@ ament_cmake - + \ No newline at end of file From 1049ee377c0b8d3e5f86ba8d24cdc8a7e92fdd2d Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Tue, 16 Apr 2024 10:02:38 -0400 Subject: [PATCH 03/54] More work commenting out things --- CMakeLists.txt | 3 +- .../robot_body_filter/RayCastingShapeMask.h | 2 +- include/robot_body_filter/RobotBodyFilter.h | 27 +- include/robot_body_filter/utils/bodies.h | 6 +- .../robot_body_filter/utils/cloud-impl.hpp | 16 +- include/robot_body_filter/utils/cloud.h | 12 +- .../robot_body_filter/utils/filter_utils.hpp | 690 +++++++++--------- .../robot_body_filter/utils/string_utils.hpp | 102 +-- src/RobotBodyFilter.cpp | 8 +- 9 files changed, 433 insertions(+), 433 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9024cb..2c6f7a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS # roscpp replaced by rclcpp rclcpp sensor_msgs + std_srvs std_msgs tf2 tf2_ros @@ -62,7 +63,7 @@ include_directories( ${Dependency}_INCLUDE_DIRS ) -add_executable(${PROJECT_NAME}_filter src/RobotBodyFilter.cpp) +add_library(${PROJECT_NAME}_filter src/RobotBodyFilter.cpp) ament_target_dependencies(${PROJECT_NAME}_filter ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_include_directories(${PROJECT_NAME}_filter diff --git a/include/robot_body_filter/RayCastingShapeMask.h b/include/robot_body_filter/RayCastingShapeMask.h index 6c86097..c628245 100644 --- a/include/robot_body_filter/RayCastingShapeMask.h +++ b/include/robot_body_filter/RayCastingShapeMask.h @@ -10,7 +10,7 @@ #include #include #include -#include +// #include namespace robot_body_filter { diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 936490f..7d50e87 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -27,17 +28,17 @@ #include // #include #include -#include +// #include #include -#include -#include -#include -#include -#include +// #include +#include +// #include +// #include +// #include #include -#include +// #include -#include +// #include namespace robot_body_filter { /** @@ -96,7 +97,7 @@ static const std::string BBOX_SUFFIX = "::bounding_box"; * \author Martin Pecka */ template -class RobotBodyFilter : public ::robot_body_filter::FilterBase { +class RobotBodyFilter : public filters::FilterBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -111,8 +112,8 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { protected: //! Handle of the node this filter runs in. - ros::NodeHandle nodeHandle; - ros::NodeHandle privateNodeHandle; + rclcpp::Node nodeHandle; + rclcpp::Node privateNodeHandle; /** \brief If true, suppose that every point in the scan was captured at a * different time instant. Otherwise, the scan is assumed to be taken at once. @@ -137,7 +138,7 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { * the masking algorithm a little bit less precise but more computationally * affordable. */ - ros::Duration modelPoseUpdateInterval; + rclcpp::Duration modelPoseUpdateInterval; /** \brief Fixed frame wrt the sensor frame. * Usually base_link for stationary robots (or sensor frame if both @@ -329,7 +330,7 @@ class RobotBodyFilter : public ::robot_body_filter::FilterBase { std::shared_ptr modelMutex; //! tf buffer length - ros::Duration tfBufferLength; + rclcpp::Duration tfBufferLength; //! tf client std::shared_ptr tfBuffer; //! tf listener diff --git a/include/robot_body_filter/utils/bodies.h b/include/robot_body_filter/utils/bodies.h index 1edbda7..f02ab8b 100644 --- a/include/robot_body_filter/utils/bodies.h +++ b/include/robot_body_filter/utils/bodies.h @@ -7,16 +7,16 @@ #if __has_include() #include #else -#error not found. Please, update geometric_shapes library to version 0.6.5+ or 0.7.4+ +// #error not found. Please, update geometric_shapes library to version 0.6.5+ or 0.7.4+ #endif -#include +// #include namespace bodies { typedef bodies::AABB AxisAlignedBoundingBox; -typedef bodies::OBB OrientedBoundingBox; +// typedef bodies::OBB OrientedBoundingBox; /** \brief Compute AABB for the body at different pose. Can't use setPose() because we want `body` to be const. */ void computeBoundingBoxAt(const bodies::Body* body, AxisAlignedBoundingBox& bbox, const Eigen::Isometry3d& pose); diff --git a/include/robot_body_filter/utils/cloud-impl.hpp b/include/robot_body_filter/utils/cloud-impl.hpp index 2dcde04..e7b20fa 100644 --- a/include/robot_body_filter/utils/cloud-impl.hpp +++ b/include/robot_body_filter/utils/cloud-impl.hpp @@ -1,7 +1,7 @@ #pragma once -#include -#include +#include +#include namespace robot_body_filter { @@ -22,19 +22,19 @@ class GenericCloudIteratorBase : public sensor_msgs::impl::PointCloud2IteratorBa template class GenericCloudConstIterator - : public GenericCloudIteratorBase { + : public GenericCloudIteratorBase { public: - GenericCloudConstIterator(const sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) : - GenericCloudIteratorBase::GenericCloudIteratorBase( + GenericCloudConstIterator(const sensor_msgs::msg::PointCloud2 &cloud_msg, const std::string &field_name) : + GenericCloudIteratorBase::GenericCloudIteratorBase( cloud_msg, field_name) {} }; template class GenericCloudIterator - : public GenericCloudIteratorBase { + : public GenericCloudIteratorBase { public: - GenericCloudIterator(sensor_msgs::PointCloud2 &cloud_msg, const std::string &field_name) : - GenericCloudIteratorBase::GenericCloudIteratorBase( + GenericCloudIterator(sensor_msgs::msg::PointCloud2 &cloud_msg, const std::string &field_name) : + GenericCloudIteratorBase::GenericCloudIteratorBase( cloud_msg, field_name) {} void copyData(const GenericCloudConstIterator &otherIter) const; diff --git a/include/robot_body_filter/utils/cloud.h b/include/robot_body_filter/utils/cloud.h index 22ed605..2d3154a 100644 --- a/include/robot_body_filter/utils/cloud.h +++ b/include/robot_body_filter/utils/cloud.h @@ -8,14 +8,14 @@ #include -#include -#include +#include #include +#include namespace robot_body_filter { -typedef sensor_msgs::PointCloud2 Cloud; +typedef sensor_msgs::msg::PointCloud2 Cloud; typedef sensor_msgs::PointCloud2Iterator CloudIter; typedef sensor_msgs::PointCloud2Iterator CloudIndexIter; typedef sensor_msgs::PointCloud2ConstIterator CloudConstIter; @@ -111,7 +111,7 @@ bool hasField(const Cloud& cloud, const std::string& fieldName); * @return Reference to the field. * @throws std::runtime_error if the field doesn't exist. */ -sensor_msgs::PointField& getField(Cloud& cloud, const std::string& fieldName); +sensor_msgs::msg::PointField& getField(Cloud& cloud, const std::string& fieldName); /** * Return the sensor_msgs::PointField with the given name. @@ -120,7 +120,7 @@ sensor_msgs::PointField& getField(Cloud& cloud, const std::string& fieldName); * @return Reference to the field. * @throws std::runtime_error if the field doesn't exist. */ -const sensor_msgs::PointField& getField(const Cloud& cloud, const std::string& fieldName); +const sensor_msgs::msg::PointField& getField(const Cloud& cloud, const std::string& fieldName); /** * Return the size (in bytes) of a sensor_msgs::PointField datatype. @@ -136,7 +136,7 @@ size_t sizeOfPointField(int datatype); * @return Size of the data. * @throws std::runtime_error if wrong datatype is passed. */ -size_t sizeOfPointField(const sensor_msgs::PointField& field); +size_t sizeOfPointField(const sensor_msgs::msg::PointField& field); /** * Copy data belonging to the given field from `in` cloud to `out` cloud. diff --git a/include/robot_body_filter/utils/filter_utils.hpp b/include/robot_body_filter/utils/filter_utils.hpp index 2231185..93db161 100644 --- a/include/robot_body_filter/utils/filter_utils.hpp +++ b/include/robot_body_filter/utils/filter_utils.hpp @@ -1,18 +1,14 @@ #ifndef ROBOT_BODY_FILTER_UTILS_FILTER_UTILS_HPP #define ROBOT_BODY_FILTER_UTILS_FILTER_UTILS_HPP -#include -#include +// #include +// #include #include -#if ROS_VERSION_MINIMUM(1, 15, 0) -#include -#else #include -#endif #include "robot_body_filter/utils/string_utils.hpp" -#include "robot_body_filter/utils/xmlrpc_traits.h" +// #include "robot_body_filter/utils/xmlrpc_traits.h" namespace robot_body_filter { @@ -24,370 +20,370 @@ namespace * provided by filters::FilterBase::getParam(), which is normally protected. We use this * workaround to avoid copy-pasting the code here. */ -template -class FilterParamHelper : public filters::FilterBase -{ -public: - FilterParamHelper(const std::string& key, const XmlRpc::XmlRpcValue& value) - { - this->params_[key] = value[key]; - } - template bool getParamHelper(const std::string& name, T& value) const - { - return filters::FilterBase::getParam(name, value); - } - bool update(const F& data_in, F& data_out) override {return false;} -protected: - bool configure() override {return false;} -}; -} +// template +// class FilterParamHelper : public filters::FilterBase +// { +// public: +// FilterParamHelper(const std::string& key, const XmlRpc::XmlRpcValue& value) +// { +// this->params_[key] = value[key]; +// } +// template bool getParamHelper(const std::string& name, T& value) const +// { +// return filters::FilterBase::getParam(name, value); +// } +// bool update(const F& data_in, F& data_out) override {return false;} +// protected: +// bool configure() override {return false;} +// }; +// } -template -class FilterBase : public filters::FilterBase -{ +// template +// class FilterBase : public filters::FilterBase +// { -protected: +// protected: - /** \brief Type of function that converts anything to a string. */ - template using ToStringFn = std::string (*)(const T&); +// /** \brief Type of function that converts anything to a string. */ +// template using ToStringFn = std::string (*)(const T&); - /** - * \brief Get the value of the given filter parameter, falling back to the - * specified default value, and print out a ROS info/warning message with - * the loaded values. - * \tparam T Param type. - * \param name Name of the parameter. If the name contains slashes and the full name is not found, - * a "recursive" search is tried using the parts of the name separated by slashes. - * This is useful if the filter config isn't loaded via a filterchain config, but via - * a dict loaded directly to ROS parameter server. - * \param defaultValue The default value to use. - * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the - * messages more informative. - * \param defaultUsed Whether the default value was used. - * \param valueToStringFn Function that converts valid/default values to string (for console - * logging). Set to nullptr to disable logging. - * \return The loaded param value. - */ - template - T getParamVerbose(const std::string &name, const T &defaultValue = T(), - const std::string &unit = "", bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - T value; - if (filters::FilterBase::getParam(name, value)) - { - if (valueToStringFn != nullptr) - { - ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << - ", value: " << valueToStringFn(value) << - prependIfNonEmpty(unit, " ")); - } - if (defaultUsed != nullptr) - *defaultUsed = false; - return value; - } - else if (this->params_.find(name) != this->params_.end()) - { // the parameter was found, but has a wrong type - ROS_ERROR_STREAM(this->getName() << ": Parameter " << name << " found, " - "but its value has a wrong type. Expected XmlRpc type " << - XmlRpcTraits::stringType << ", got type: " << - to_string(this->params_.at(name).getType()) << - ". Using the default value instead."); - } - else if (name.length() > 1 && name.find_first_of('/', 1) != std::string::npos) - { // The parameter has slashes in its name, so try a "recursive" search - auto slashPos = name.find_first_of('/', 1); - auto head = name.substr(0, slashPos); - auto tail = name.substr(slashPos + 1); - XmlRpc::XmlRpcValue val; +// /** +// * \brief Get the value of the given filter parameter, falling back to the +// * specified default value, and print out a ROS info/warning message with +// * the loaded values. +// * \tparam T Param type. +// * \param name Name of the parameter. If the name contains slashes and the full name is not found, +// * a "recursive" search is tried using the parts of the name separated by slashes. +// * This is useful if the filter config isn't loaded via a filterchain config, but via +// * a dict loaded directly to ROS parameter server. +// * \param defaultValue The default value to use. +// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the +// * messages more informative. +// * \param defaultUsed Whether the default value was used. +// * \param valueToStringFn Function that converts valid/default values to string (for console +// * logging). Set to nullptr to disable logging. +// * \return The loaded param value. +// */ +// template +// T getParamVerbose(const std::string &name, const T &defaultValue = T(), +// const std::string &unit = "", bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// T value; +// if (filters::FilterBase::getParam(name, value)) +// { +// if (valueToStringFn != nullptr) +// { +// ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << +// ", value: " << valueToStringFn(value) << +// prependIfNonEmpty(unit, " ")); +// } +// if (defaultUsed != nullptr) +// *defaultUsed = false; +// return value; +// } +// else if (this->params_.find(name) != this->params_.end()) +// { // the parameter was found, but has a wrong type +// ROS_ERROR_STREAM(this->getName() << ": Parameter " << name << " found, " +// "but its value has a wrong type. Expected XmlRpc type " << +// XmlRpcTraits::stringType << ", got type: " << +// to_string(this->params_.at(name).getType()) << +// ". Using the default value instead."); +// } +// else if (name.length() > 1 && name.find_first_of('/', 1) != std::string::npos) +// { // The parameter has slashes in its name, so try a "recursive" search +// auto slashPos = name.find_first_of('/', 1); +// auto head = name.substr(0, slashPos); +// auto tail = name.substr(slashPos + 1); +// XmlRpc::XmlRpcValue val; - if (filters::FilterBase::getParam(head, val)) - { - while (val.getType() == XmlRpc::XmlRpcValue::TypeStruct) - { - if (val.hasMember(tail)) - { - auto tmpFilter = FilterParamHelper(tail, val); - if (tmpFilter.getParamHelper(tail, value)) - { - if (defaultUsed != nullptr) - *defaultUsed = false; - if (valueToStringFn != nullptr) - { - ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << - ", value: " << valueToStringFn(value) << prependIfNonEmpty(unit, " ")); - } - return value; - } - else - { - ROS_ERROR_STREAM(this->getName() << ": Parameter " << name << " found, " - "but its value has a wrong type. Expected XmlRpc type " << - XmlRpcTraits::stringType << ", got type: " << - to_string(val[tail].getType()) << - ". Using the default value instead."); - break; - } - } else { - slashPos = tail.find_first_of('/', 1); - if (slashPos == std::string::npos) - break; - head = tail.substr(0, slashPos); - tail = tail.substr(slashPos + 1); - if (!val.hasMember(head)) - break; - XmlRpc::XmlRpcValue tmp = val[head]; // tmp copy is required, otherwise mem corruption - val = tmp; - if (!val.valid()) - break; - } - } - } - } +// if (filters::FilterBase::getParam(head, val)) +// { +// while (val.getType() == XmlRpc::XmlRpcValue::TypeStruct) +// { +// if (val.hasMember(tail)) +// { +// auto tmpFilter = FilterParamHelper(tail, val); +// if (tmpFilter.getParamHelper(tail, value)) +// { +// if (defaultUsed != nullptr) +// *defaultUsed = false; +// if (valueToStringFn != nullptr) +// { +// ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << +// ", value: " << valueToStringFn(value) << prependIfNonEmpty(unit, " ")); +// } +// return value; +// } +// else +// { +// ROS_ERROR_STREAM(this->getName() << ": Parameter " << name << " found, " +// "but its value has a wrong type. Expected XmlRpc type " << +// XmlRpcTraits::stringType << ", got type: " << +// to_string(val[tail].getType()) << +// ". Using the default value instead."); +// break; +// } +// } else { +// slashPos = tail.find_first_of('/', 1); +// if (slashPos == std::string::npos) +// break; +// head = tail.substr(0, slashPos); +// tail = tail.substr(slashPos + 1); +// if (!val.hasMember(head)) +// break; +// XmlRpc::XmlRpcValue tmp = val[head]; // tmp copy is required, otherwise mem corruption +// val = tmp; +// if (!val.valid()) +// break; +// } +// } +// } +// } - if (valueToStringFn != nullptr) - { - ROS_INFO_STREAM(this->getName() << ": Parameter " << name - << " not defined, assigning default: " - << valueToStringFn(defaultValue) - << prependIfNonEmpty(unit, " ")); - } - if (defaultUsed != nullptr) - *defaultUsed = true; - return defaultValue; - } +// if (valueToStringFn != nullptr) +// { +// ROS_INFO_STREAM(this->getName() << ": Parameter " << name +// << " not defined, assigning default: " +// << valueToStringFn(defaultValue) +// << prependIfNonEmpty(unit, " ")); +// } +// if (defaultUsed != nullptr) +// *defaultUsed = true; +// return defaultValue; +// } - /** \brief Get the value of the given filter parameter, falling back to the - * specified default value, and print out a ROS info/warning message with - * the loaded values. - * \param name Name of the parameter. If the name contains slashes and the full name is not found, - * a "recursive" search is tried using the parts of the name separated by slashes. - * This is useful if the filter config isn't loaded via a filterchain config, but via - * a dict loaded directly to ROS parameter server. - * \param defaultValue The default value to use. - * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the - * messages more informative. - * \return The loaded param value. - */ - std::string getParamVerbose(const std::string &name, const char* defaultValue, - const std::string &unit = "", bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - return this->getParamVerbose(name, std::string(defaultValue), unit, defaultUsed, valueToStringFn); - } +// /** \brief Get the value of the given filter parameter, falling back to the +// * specified default value, and print out a ROS info/warning message with +// * the loaded values. +// * \param name Name of the parameter. If the name contains slashes and the full name is not found, +// * a "recursive" search is tried using the parts of the name separated by slashes. +// * This is useful if the filter config isn't loaded via a filterchain config, but via +// * a dict loaded directly to ROS parameter server. +// * \param defaultValue The default value to use. +// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the +// * messages more informative. +// * \return The loaded param value. +// */ +// std::string getParamVerbose(const std::string &name, const char* defaultValue, +// const std::string &unit = "", bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// return this->getParamVerbose(name, std::string(defaultValue), unit, defaultUsed, valueToStringFn); +// } - // getParam specializations for unsigned values +// // getParam specializations for unsigned values - /** \brief Get the value of the given filter parameter, falling back to the - * specified default value, and print out a ROS info/warning message with - * the loaded values. - * \param name Name of the parameter. If the name contains slashes and the full name is not found, - * a "recursive" search is tried using the parts of the name separated by slashes. - * This is useful if the filter config isn't loaded via a filterchain config, but via - * a dict loaded directly to ROS parameter server. - * \param defaultValue The default value to use. - * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the - * messages more informative. - * \return The loaded param value. - * \throw std::invalid_argument If the loaded value is negative. - */ - uint64_t getParamVerbose(const std::string &name, const uint64_t &defaultValue, - const std::string &unit = "", bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - return this->getParamUnsigned(name, defaultValue, unit, defaultUsed, - valueToStringFn); - } +// /** \brief Get the value of the given filter parameter, falling back to the +// * specified default value, and print out a ROS info/warning message with +// * the loaded values. +// * \param name Name of the parameter. If the name contains slashes and the full name is not found, +// * a "recursive" search is tried using the parts of the name separated by slashes. +// * This is useful if the filter config isn't loaded via a filterchain config, but via +// * a dict loaded directly to ROS parameter server. +// * \param defaultValue The default value to use. +// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the +// * messages more informative. +// * \return The loaded param value. +// * \throw std::invalid_argument If the loaded value is negative. +// */ +// uint64_t getParamVerbose(const std::string &name, const uint64_t &defaultValue, +// const std::string &unit = "", bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// return this->getParamUnsigned(name, defaultValue, unit, defaultUsed, +// valueToStringFn); +// } - // there actually is an unsigned int implementation of FilterBase::getParam, - // but it doesn't tell you when the passed value is negative - instead it just - // returns false - /** \brief Get the value of the given filter parameter, falling back to the - * specified default value, and print out a ROS info/warning message with - * the loaded values. - * \param name Name of the parameter. If the name contains slashes and the full name is not found, - * a "recursive" search is tried using the parts of the name separated by slashes. - * This is useful if the filter config isn't loaded via a filterchain config, but via - * a dict loaded directly to ROS parameter server. - * \param defaultValue The default value to use. - * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the - * messages more informative. - * \return The loaded param value. - * \throw std::invalid_argument If the loaded value is negative. - */ - unsigned int getParamVerbose(const std::string &name, - const unsigned int &defaultValue, - const std::string &unit = "", - bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - return this->getParamUnsigned(name, defaultValue, unit, defaultUsed, - valueToStringFn); - } +// // there actually is an unsigned int implementation of FilterBase::getParam, +// // but it doesn't tell you when the passed value is negative - instead it just +// // returns false +// /** \brief Get the value of the given filter parameter, falling back to the +// * specified default value, and print out a ROS info/warning message with +// * the loaded values. +// * \param name Name of the parameter. If the name contains slashes and the full name is not found, +// * a "recursive" search is tried using the parts of the name separated by slashes. +// * This is useful if the filter config isn't loaded via a filterchain config, but via +// * a dict loaded directly to ROS parameter server. +// * \param defaultValue The default value to use. +// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the +// * messages more informative. +// * \return The loaded param value. +// * \throw std::invalid_argument If the loaded value is negative. +// */ +// unsigned int getParamVerbose(const std::string &name, +// const unsigned int &defaultValue, +// const std::string &unit = "", +// bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// return this->getParamUnsigned(name, defaultValue, unit, defaultUsed, +// valueToStringFn); +// } - // ROS types specializations +// // ROS types specializations - /** \brief Get the value of the given filter parameter, falling back to the - * specified default value, and print out a ROS info/warning message with - * the loaded values. - * \param name Name of the parameter. If the name contains slashes and the full name is not found, - * a "recursive" search is tried using the parts of the name separated by slashes. - * This is useful if the filter config isn't loaded via a filterchain config, but via - * a dict loaded directly to ROS parameter server. - * \param defaultValue The default value to use. - * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the - * messages more informative. - * \return The loaded param value. - */ - ros::Duration getParamVerbose(const std::string &name, - const ros::Duration &defaultValue, - const std::string &unit = "", - bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - return this->getParamCast(name, defaultValue.toSec(), unit, defaultUsed, - valueToStringFn); - } +// /** \brief Get the value of the given filter parameter, falling back to the +// * specified default value, and print out a ROS info/warning message with +// * the loaded values. +// * \param name Name of the parameter. If the name contains slashes and the full name is not found, +// * a "recursive" search is tried using the parts of the name separated by slashes. +// * This is useful if the filter config isn't loaded via a filterchain config, but via +// * a dict loaded directly to ROS parameter server. +// * \param defaultValue The default value to use. +// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the +// * messages more informative. +// * \return The loaded param value. +// */ +// ros::Duration getParamVerbose(const std::string &name, +// const ros::Duration &defaultValue, +// const std::string &unit = "", +// bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// return this->getParamCast(name, defaultValue.toSec(), unit, defaultUsed, +// valueToStringFn); +// } - /** \brief Get the value of the given filter parameter as a set of strings, falling back to the - * specified default value, and print out a ROS info/warning message with - * the loaded values. - * \tparam T Type of the values in the set. Only std::string and double are supported. - * \param name Name of the parameter. If the name contains slashes and the full name is not found, - * a "recursive" search is tried using the parts of the name separated by slashes. - * This is useful if the filter config isn't loaded via a filterchain config, but via - * a dict loaded directly to ROS parameter server. - * \param defaultValue The default value to use. - * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the - * messages more informative. - * \return The loaded param value. - */ - template - std::set getParamVerboseSet( - const std::string &name, - const std::set &defaultValue = std::set(), - const std::string &unit = "", - bool* defaultUsed = nullptr, - ToStringFn> valueToStringFn = &to_string) const - { - std::vector vector(defaultValue.begin(), defaultValue.end()); - vector = this->getParamVerbose(name, vector, unit, defaultUsed, valueToStringFn); - return std::set(vector.begin(), vector.end()); - } +// /** \brief Get the value of the given filter parameter as a set of strings, falling back to the +// * specified default value, and print out a ROS info/warning message with +// * the loaded values. +// * \tparam T Type of the values in the set. Only std::string and double are supported. +// * \param name Name of the parameter. If the name contains slashes and the full name is not found, +// * a "recursive" search is tried using the parts of the name separated by slashes. +// * This is useful if the filter config isn't loaded via a filterchain config, but via +// * a dict loaded directly to ROS parameter server. +// * \param defaultValue The default value to use. +// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the +// * messages more informative. +// * \return The loaded param value. +// */ +// template +// std::set getParamVerboseSet( +// const std::string &name, +// const std::set &defaultValue = std::set(), +// const std::string &unit = "", +// bool* defaultUsed = nullptr, +// ToStringFn> valueToStringFn = &to_string) const +// { +// std::vector vector(defaultValue.begin(), defaultValue.end()); +// vector = this->getParamVerbose(name, vector, unit, defaultUsed, valueToStringFn); +// return std::set(vector.begin(), vector.end()); +// } - template> - MapType getParamVerboseMap( - const std::string &name, - const std::map &defaultValue = std::map(), - const std::string &unit = "", - bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - // convert default value to XmlRpc so that we can utilize FilterBase::getParam(XmlRpcValue). - XmlRpc::XmlRpcValue defaultValueXmlRpc; - defaultValueXmlRpc.begin(); // calls assertStruct() which mutates this value into a struct - for (const auto& val : defaultValue) - defaultValueXmlRpc[val.first] = val.second; +// template> +// MapType getParamVerboseMap( +// const std::string &name, +// const std::map &defaultValue = std::map(), +// const std::string &unit = "", +// bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// // convert default value to XmlRpc so that we can utilize FilterBase::getParam(XmlRpcValue). +// XmlRpc::XmlRpcValue defaultValueXmlRpc; +// defaultValueXmlRpc.begin(); // calls assertStruct() which mutates this value into a struct +// for (const auto& val : defaultValue) +// defaultValueXmlRpc[val.first] = val.second; - // get the param value as a XmlRpcValue - bool innerDefaultUsed; - auto valueXmlRpc = this->getParamVerbose(name, defaultValueXmlRpc, unit, &innerDefaultUsed, - (ToStringFn)nullptr); +// // get the param value as a XmlRpcValue +// bool innerDefaultUsed; +// auto valueXmlRpc = this->getParamVerbose(name, defaultValueXmlRpc, unit, &innerDefaultUsed, +// (ToStringFn)nullptr); - // convert to map - MapType value; - bool hasWrongTypeItems = false; - for (auto& pairXmlRpc : valueXmlRpc) - { - if (pairXmlRpc.second.getType() == XmlRpcTraits::xmlRpcType) - { - value[pairXmlRpc.first] = pairXmlRpc.second.operator T&(); - } - else if (XmlRpcTraits::xmlRpcType == XmlRpc::XmlRpcValue::TypeDouble && pairXmlRpc.second.getType() == XmlRpc::XmlRpcValue::TypeInt) - { - // special handling of the case when doubles are expected but an int is provided - value[pairXmlRpc.first] = static_cast(pairXmlRpc.second.operator int&()); - } - else - { - ROS_WARN_STREAM(this->getName() << ": Invalid value for dict parameter " << name - << " key " << pairXmlRpc.first << ". Expected XmlRpc type " << XmlRpcTraits::stringType - << ", got type: " << to_string(pairXmlRpc.second.getType()) << ". Skipping value."); - hasWrongTypeItems = true; - } - } +// // convert to map +// MapType value; +// bool hasWrongTypeItems = false; +// for (auto& pairXmlRpc : valueXmlRpc) +// { +// if (pairXmlRpc.second.getType() == XmlRpcTraits::xmlRpcType) +// { +// value[pairXmlRpc.first] = pairXmlRpc.second.operator T&(); +// } +// else if (XmlRpcTraits::xmlRpcType == XmlRpc::XmlRpcValue::TypeDouble && pairXmlRpc.second.getType() == XmlRpc::XmlRpcValue::TypeInt) +// { +// // special handling of the case when doubles are expected but an int is provided +// value[pairXmlRpc.first] = static_cast(pairXmlRpc.second.operator int&()); +// } +// else +// { +// ROS_WARN_STREAM(this->getName() << ": Invalid value for dict parameter " << name +// << " key " << pairXmlRpc.first << ". Expected XmlRpc type " << XmlRpcTraits::stringType +// << ", got type: " << to_string(pairXmlRpc.second.getType()) << ". Skipping value."); +// hasWrongTypeItems = true; +// } +// } - if (value.empty() && hasWrongTypeItems) - { - value = defaultValue; - if (defaultUsed != nullptr) - *defaultUsed = true; - if (valueToStringFn != nullptr) - { - ROS_ERROR_STREAM(this->getName() << ": Dict parameter " << name - << " got only invalid types of values, assigning default: " - << valueToStringFn(defaultValue) - << prependIfNonEmpty(unit, " ")); - } - } else { - if (defaultUsed != nullptr) - *defaultUsed = innerDefaultUsed; - if (valueToStringFn != nullptr) - { - if (innerDefaultUsed) - { - ROS_INFO_STREAM(this->getName() << ": Parameter " << name - << " not defined, assigning default: " - << valueToStringFn(defaultValue) - << prependIfNonEmpty(unit, " ")); - } - else - { - ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << - ", value: " << valueToStringFn(value) << - prependIfNonEmpty(unit, " ")); - } - } - } +// if (value.empty() && hasWrongTypeItems) +// { +// value = defaultValue; +// if (defaultUsed != nullptr) +// *defaultUsed = true; +// if (valueToStringFn != nullptr) +// { +// ROS_ERROR_STREAM(this->getName() << ": Dict parameter " << name +// << " got only invalid types of values, assigning default: " +// << valueToStringFn(defaultValue) +// << prependIfNonEmpty(unit, " ")); +// } +// } else { +// if (defaultUsed != nullptr) +// *defaultUsed = innerDefaultUsed; +// if (valueToStringFn != nullptr) +// { +// if (innerDefaultUsed) +// { +// ROS_INFO_STREAM(this->getName() << ": Parameter " << name +// << " not defined, assigning default: " +// << valueToStringFn(defaultValue) +// << prependIfNonEmpty(unit, " ")); +// } +// else +// { +// ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << +// ", value: " << valueToStringFn(value) << +// prependIfNonEmpty(unit, " ")); +// } +// } +// } - return value; - } +// return value; +// } -private: +// private: - template - Result getParamUnsigned(const std::string &name, const Result &defaultValue, - const std::string &unit = "", bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - const Param signedValue = this->getParamVerbose(name, static_cast(defaultValue), unit, - defaultUsed, valueToStringFn); - if (signedValue < 0) - { - if (valueToStringFn != nullptr) - { - ROS_ERROR_STREAM(this->getName() << ": Value " << valueToStringFn(signedValue) << - " of unsigned parameter " << name << " is negative."); - } - throw std::invalid_argument(name); - } - return static_cast(signedValue); - } +// template +// Result getParamUnsigned(const std::string &name, const Result &defaultValue, +// const std::string &unit = "", bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// const Param signedValue = this->getParamVerbose(name, static_cast(defaultValue), unit, +// defaultUsed, valueToStringFn); +// if (signedValue < 0) +// { +// if (valueToStringFn != nullptr) +// { +// ROS_ERROR_STREAM(this->getName() << ": Value " << valueToStringFn(signedValue) << +// " of unsigned parameter " << name << " is negative."); +// } +// throw std::invalid_argument(name); +// } +// return static_cast(signedValue); +// } - // generic casting getParam() - template - Result getParamCast(const std::string &name, const Param &defaultValue, - const std::string &unit = "", bool* defaultUsed = nullptr, - ToStringFn valueToStringFn = &to_string) const - { - const Param paramValue = this->getParamVerbose(name, defaultValue, unit, defaultUsed, - valueToStringFn); - return Result(paramValue); - } +// // generic casting getParam() +// template +// Result getParamCast(const std::string &name, const Param &defaultValue, +// const std::string &unit = "", bool* defaultUsed = nullptr, +// ToStringFn valueToStringFn = &to_string) const +// { +// const Param paramValue = this->getParamVerbose(name, defaultValue, unit, defaultUsed, +// valueToStringFn); +// return Result(paramValue); +// } -}; +// }; } #endif //ROBOT_BODY_FILTER_UTILS_FILTER_UTILS_HPP diff --git a/include/robot_body_filter/utils/string_utils.hpp b/include/robot_body_filter/utils/string_utils.hpp index b579b09..76bc8fd 100644 --- a/include/robot_body_filter/utils/string_utils.hpp +++ b/include/robot_body_filter/utils/string_utils.hpp @@ -3,14 +3,16 @@ #include #include -#include -#include +// #include +// #include +#include +#include #include #include #include #include #include -#include +// #include namespace robot_body_filter { @@ -97,12 +99,12 @@ inline std::string to_string(const std::string &value) return value; } -template<> -inline std::string to_string(const XmlRpc::XmlRpcValue &value) -{ - std::stringstream ss; - ss << value; - return ss.str(); +// template<> +// inline std::string to_string(const XmlRpc::XmlRpcValue &value) +// { +// std::stringstream ss; +// ss << value; +// return ss.str(); } template @@ -171,45 +173,45 @@ inline std::string to_string(const std::map &value) return ss.str(); } -template<> -inline std::string to_string(const ros::Time& value) -{ - std::stringstream ss; - ss << value; - return ss.str(); -} - -template<> -inline std::string to_string(const ros::WallTime& value) -{ - std::stringstream ss; - ss << value; - return ss.str(); -} - -template<> -inline std::string to_string(const ros::SteadyTime& value) -{ - std::stringstream ss; - ss << value; - return ss.str(); -} - -template<> -inline std::string to_string(const ros::Duration& value) -{ - std::stringstream ss; - ss << value; - return ss.str(); -} - -template<> -inline std::string to_string(const ros::WallDuration& value) -{ - std::stringstream ss; - ss << value; - return ss.str(); -} - -}; +// template<> +// inline std::string to_string(const rclcpp::Time& value) +// { +// std::stringstream ss; +// ss << value; +// return ss.str(); +// } + +// template<> +// inline std::string to_string(const rclcpp::WallTime& value) +// { +// std::stringstream ss; +// ss << value; +// return ss.str(); +// } + +// template<> +// inline std::string to_string(const rclcpp::SteadyTime& value) +// { +// std::stringstream ss; +// ss << value; +// return ss.str(); +// } + +// template<> +// inline std::string to_string(const rclcpp::Duration& value) +// { +// std::stringstream ss; +// ss << value; +// return ss.str(); +// } + +// template<> +// inline std::string to_string(const rclcpp::WallDuration& value) +// { +// std::stringstream ss; +// ss << value; +// return ss.str(); +// } + +// }; #endif //ROBOT_BODY_FILTER_UTILS_TOPIC_UTILS_HPP \ No newline at end of file diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index e88424a..f7f2971 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -30,9 +30,9 @@ #include #include -using namespace std; -using namespace sensor_msgs; -using namespace filters; +// using namespace std; +// using namespace sensor_msgs; +// using namespace filters; namespace robot_body_filter { @@ -43,7 +43,7 @@ RobotBodyFilter::RobotBodyFilter() : privateNodeHandle("~") { template bool RobotBodyFilter::configure() { - this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", ros::Duration(60.0), "s"); + this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", rclcpp::Duration(60.0), "s"); if (this->tfBuffer == nullptr) { From 1f75d92d8e39a9e29c6e94af590f0410b965760e Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Tue, 16 Apr 2024 10:18:45 -0400 Subject: [PATCH 04/54] More commenting, Almost compiles, just need to translate subscribers into publishers --- CMakeLists.txt | 2 +- include/robot_body_filter/RobotBodyFilter.h | 159 ++--- include/robot_body_filter/TfFramesWatchdog.h | 16 +- src/RobotBodyFilter.cpp | 680 +++++++++---------- 4 files changed, 429 insertions(+), 428 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c6f7a1..430a7c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ cmake_minimum_required(VERSION 3.5) project(robot_body_filter) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 7d50e87..5eef8b4 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -13,32 +13,33 @@ #include +#include // #include // #include -#include +// #include // #include // #include // #include -#include -#include -#include +// #include +// #include +// #include #include -#include -#include +// #include +// #include #include // #include #include // #include -#include +// #include // #include #include -// #include -// #include -// #include +#include +#include +#include #include -// #include +#include -// #include +#include namespace robot_body_filter { /** @@ -216,7 +217,7 @@ class RobotBodyFilter : public filters::FilterBase { std::string robotDescriptionParam; //! Subscriber for robot_description updates. - ros::Subscriber robotDescriptionUpdatesListener; + // ros::Subscriber robotDescriptionUpdatesListener; //! Name of the field in the dynamic reconfigure message that contains robot //! model. std::string robotDescriptionUpdatesFieldName; @@ -229,48 +230,48 @@ class RobotBodyFilter : public filters::FilterBase { std::set onlyLinks; //! Publisher of robot bounding sphere (relative to fixed frame). - ros::Publisher boundingSpherePublisher; - //! Publisher of robot bounding box (relative to fixed frame). - ros::Publisher boundingBoxPublisher; - //! Publisher of robot bounding box (relative to fixed frame). - ros::Publisher orientedBoundingBoxPublisher; - //! Publisher of robot bounding box (relative to defined local frame). - ros::Publisher localBoundingBoxPublisher; - //! Publisher of the bounding sphere marker. - ros::Publisher boundingSphereMarkerPublisher; - //! Publisher of the bounding box marker. - ros::Publisher boundingBoxMarkerPublisher; - //! Publisher of the oriented bounding box marker. - ros::Publisher orientedBoundingBoxMarkerPublisher; - //! Publisher of the local bounding box marker. - ros::Publisher localBoundingBoxMarkerPublisher; - //! Publisher of the debug bounding box markers. - ros::Publisher boundingBoxDebugMarkerPublisher; - //! Publisher of the debug oriented bounding box markers. - ros::Publisher orientedBoundingBoxDebugMarkerPublisher; - //! Publisher of the debug local bounding box markers. - ros::Publisher localBoundingBoxDebugMarkerPublisher; - //! Publisher of the debug bounding sphere markers. - ros::Publisher boundingSphereDebugMarkerPublisher; - - //! Publisher of scan_point_cloud with robot bounding box cut out. - ros::Publisher scanPointCloudNoBoundingBoxPublisher; - //! Publisher of scan_point_cloud with robot oriented bounding box cut out. - ros::Publisher scanPointCloudNoOrientedBoundingBoxPublisher; - //! Publisher of scan_point_cloud with robot local bounding box cut out. - ros::Publisher scanPointCloudNoLocalBoundingBoxPublisher; - //! Publisher of scan_point_cloud with robot bounding sphere cut out. - ros::Publisher scanPointCloudNoBoundingSpherePublisher; - ros::Publisher debugPointCloudInsidePublisher; - ros::Publisher debugPointCloudClipPublisher; - ros::Publisher debugPointCloudShadowPublisher; - ros::Publisher debugContainsMarkerPublisher; - ros::Publisher debugShadowMarkerPublisher; - ros::Publisher debugBsphereMarkerPublisher; - ros::Publisher debugBboxMarkerPublisher; + // ros::Publisher boundingSpherePublisher; + // //! Publisher of robot bounding box (relative to fixed frame). + // ros::Publisher boundingBoxPublisher; + // //! Publisher of robot bounding box (relative to fixed frame). + // ros::Publisher orientedBoundingBoxPublisher; + // //! Publisher of robot bounding box (relative to defined local frame). + // ros::Publisher localBoundingBoxPublisher; + // //! Publisher of the bounding sphere marker. + // ros::Publisher boundingSphereMarkerPublisher; + // //! Publisher of the bounding box marker. + // ros::Publisher boundingBoxMarkerPublisher; + // //! Publisher of the oriented bounding box marker. + // ros::Publisher orientedBoundingBoxMarkerPublisher; + // //! Publisher of the local bounding box marker. + // ros::Publisher localBoundingBoxMarkerPublisher; + // //! Publisher of the debug bounding box markers. + // ros::Publisher boundingBoxDebugMarkerPublisher; + // //! Publisher of the debug oriented bounding box markers. + // ros::Publisher orientedBoundingBoxDebugMarkerPublisher; + // //! Publisher of the debug local bounding box markers. + // ros::Publisher localBoundingBoxDebugMarkerPublisher; + // //! Publisher of the debug bounding sphere markers. + // ros::Publisher boundingSphereDebugMarkerPublisher; + + // //! Publisher of scan_point_cloud with robot bounding box cut out. + // ros::Publisher scanPointCloudNoBoundingBoxPublisher; + // //! Publisher of scan_point_cloud with robot oriented bounding box cut out. + // ros::Publisher scanPointCloudNoOrientedBoundingBoxPublisher; + // //! Publisher of scan_point_cloud with robot local bounding box cut out. + // ros::Publisher scanPointCloudNoLocalBoundingBoxPublisher; + // //! Publisher of scan_point_cloud with robot bounding sphere cut out. + // ros::Publisher scanPointCloudNoBoundingSpherePublisher; + // ros::Publisher debugPointCloudInsidePublisher; + // ros::Publisher debugPointCloudClipPublisher; + // ros::Publisher debugPointCloudShadowPublisher; + // ros::Publisher debugContainsMarkerPublisher; + // ros::Publisher debugShadowMarkerPublisher; + // ros::Publisher debugBsphereMarkerPublisher; + // ros::Publisher debugBboxMarkerPublisher; //! Service server for reloading robot model. - ros::ServiceServer reloadRobotModelServiceServer; + // ros::ServiceServer reloadRobotModelServiceServer; //! Whether to compute bounding sphere of the robot. bool computeBoundingSphere; @@ -318,9 +319,9 @@ class RobotBodyFilter : public filters::FilterBase { bool publishDebugBboxMarker; //! Timeout for reachable transforms. - ros::Duration reachableTransformTimeout; + rclcpp::Duration reachableTransformTimeout; //! Timeout for unreachable transforms. - ros::Duration unreachableTransformTimeout; + rclcpp::Duration unreachableTransformTimeout; //! Whether to process data when there are some unreachable frames. bool requireAllFramesReachable; @@ -340,7 +341,7 @@ class RobotBodyFilter : public filters::FilterBase { std::shared_ptr tfFramesWatchdog; //! The time when the filter configuration has finished. - ros::Time timeConfigured; + rclcpp::Time timeConfigured; //! Tool for masking out 3D bodies out of point clouds. std::unique_ptr shapeMask; @@ -386,7 +387,7 @@ class RobotBodyFilter : public filters::FilterBase { * sensor position from the viewpoint channels. * \return Whether the computation succeeded. */ - bool computeMask(const sensor_msgs::PointCloud2 &projectedPointCloud, + bool computeMask(const sensor_msgs::msg::PointCloud2 &projectedPointCloud, std::vector &mask, const std::string &sensorFrame = ""); @@ -423,8 +424,8 @@ class RobotBodyFilter : public filters::FilterBase { * \param afterScantime The after scan time to get transforms for (if zero * time is passed, after scan transforms are not computed). */ - void updateTransformCache(const ros::Time &time, - const ros::Time &afterScanTime = ros::Time(0)); + void updateTransformCache(const rclcpp::Time &time, + const rclcpp::Time &afterScanTime = rclcpp::Time(0)); /** * \brief Callback handling update of the robot_description parameter using @@ -432,52 +433,52 @@ class RobotBodyFilter : public filters::FilterBase { * * \param newConfig The updated config. */ - void robotDescriptionUpdated(dynamic_reconfigure::ConfigConstPtr newConfig); + // void robotDescriptionUpdated(dynamic_reconfigure::ConfigConstPtr newConfig); /** * \brief Callback for ~reload_model service. Reloads the URDF from parameter. * \return Success. */ - bool triggerModelReload(std_srvs::TriggerRequest &, - std_srvs::TriggerResponse &); + bool triggerModelReload(std_srvs::srv::Trigger_Request &, + std_srvs::srv::Trigger_Response &); void createBodyVisualizationMsg( const std::map &bodies, - const ros::Time &stamp, const std_msgs::ColorRGBA &color, - visualization_msgs::MarkerArray &markerArray) const; + const rclcpp::Time &stamp, const std_msgs::msg::ColorRGBA &color, + visualization_msgs::msg::MarkerArray &markerArray) const; - void publishDebugMarkers(const ros::Time &scanTime) const; + void publishDebugMarkers(const rclcpp::Time &scanTime) const; void publishDebugPointClouds( - const sensor_msgs::PointCloud2 &projectedPointCloud, + const sensor_msgs::msg::PointCloud2 &projectedPointCloud, const std::vector &pointMask) const; /** * \brief Computation of the bounding sphere, debug spheres, and publishing of * pointcloud without bounding sphere. */ void computeAndPublishBoundingSphere( - const sensor_msgs::PointCloud2 &projectedPointCloud) const; + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; /** * \brief Computation of the bounding box, debug boxes, and publishing of * pointcloud without bounding box. */ void computeAndPublishBoundingBox( - const sensor_msgs::PointCloud2 &projectedPointCloud) const; + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; /** * \brief Computation of the oriented bounding box, debug boxes, and * publishing of pointcloud without bounding box. */ void computeAndPublishOrientedBoundingBox( - const sensor_msgs::PointCloud2 &projectedPointCloud) const; + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; /** * \brief Computation of the local bounding box, debug boxes, and publishing * of pointcloud without bounding box. */ void computeAndPublishLocalBoundingBox( - const sensor_msgs::PointCloud2 &projectedPointCloud) const; + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; ScaleAndPadding getLinkInflationForContainsTest(const std::string &linkName) const; @@ -504,11 +505,11 @@ class RobotBodyFilter : public filters::FilterBase { }; class RobotBodyFilterLaserScan - : public RobotBodyFilter { + : public filters::FilterBase { public: //! Apply the filter. - bool update(const sensor_msgs::LaserScan &inputScan, - sensor_msgs::LaserScan &filteredScan) override; + bool update(const sensor_msgs::msg::LaserScan &inputScan, + sensor_msgs::msg::LaserScan &filteredScan) override; bool configure() override; @@ -517,16 +518,16 @@ class RobotBodyFilterLaserScan // in RobotBodyFilterLaserScan::update we project the scan to a pointcloud // with viewpoints - const std::unordered_map channelsToTransform{ - {"vp_", CloudChannelType::POINT}}; + // const std::unordered_map channelsToTransform{ + // {"vp_", CloudChannelType::POINT}}; }; class RobotBodyFilterPointCloud2 - : public RobotBodyFilter { + : public filters::FilterBase { public: //! Apply the filter. - bool update(const sensor_msgs::PointCloud2 &inputCloud, - sensor_msgs::PointCloud2 &filteredCloud) override; + bool update(const sensor_msgs::msg::PointCloud2 &inputCloud, + sensor_msgs::msg::PointCloud2 &filteredCloud) override; bool configure() override; @@ -534,7 +535,7 @@ class RobotBodyFilterPointCloud2 /** \brief Frame into which the output data should be transformed. */ std::string outputFrame; - std::unordered_map channelsToTransform; + // std::unordered_map channelsToTransform; }; } // namespace robot_body_filter diff --git a/include/robot_body_filter/TfFramesWatchdog.h b/include/robot_body_filter/TfFramesWatchdog.h index bcc6373..6023800 100644 --- a/include/robot_body_filter/TfFramesWatchdog.h +++ b/include/robot_body_filter/TfFramesWatchdog.h @@ -6,7 +6,7 @@ #include #include -#include +// #include #include #include @@ -28,8 +28,8 @@ class TFFramesWatchdog { TFFramesWatchdog(std::string robotFrame, std::set monitoredFrames, std::shared_ptr tfBuffer, - ros::Duration unreachableTfLookupTimeout = ros::Duration(0, 100000000), // 0.1 sec - ros::Rate unreachableFramesCheckRate = ros::Rate(1.0)); + rclcpp::Duration unreachableTfLookupTimeout = rclcpp::Duration(0, 100000000), // 0.1 sec + rclcpp::Rate unreachableFramesCheckRate = rclcpp::Rate(1.0)); virtual ~TFFramesWatchdog(); @@ -114,10 +114,10 @@ class TFFramesWatchdog { * \throws std::runtime_exception If you call this function before a call to * start(). */ - optional lookupTransform( + optional lookupTransform( const std::string& frame, - const ros::Time& time, - const ros::Duration& timeout, + const rclcpp::Time& time, + const rclcpp::Duration& timeout, std::string* errstr = nullptr); protected: @@ -181,9 +181,9 @@ class TFFramesWatchdog { std::shared_ptr tfBuffer; //! Timeout for canTransform() for figuring out if an unreachable frame became reachable. - ros::Duration unreachableTfLookupTimeout; + rclcpp::Duration unreachableTfLookupTimeout; //! Rate at which checking for unreachable frames will be done. - ros::Rate unreachableFramesCheckRate; + rclcpp::Rate unreachableFramesCheckRate; //! Lock this mutex any time you want to work with monitoredFrames or reachableFrames. mutable std::mutex framesMutex; diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index f7f2971..f5af32b 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -5,15 +5,15 @@ #include "robot_body_filter/RobotBodyFilter.h" -#include "pluginlib/class_list_macros.h" +// #include "pluginlib/class_list_macros.h" #include #include #include #include -#include -#include +#include +#include #include #include @@ -25,8 +25,8 @@ #include #include #include -#include -#include +// #include +// #include #include #include @@ -54,364 +54,364 @@ bool RobotBodyFilter::configure() { this->tfBuffer->clear(); } - this->fixedFrame = this->getParamVerbose("frames/fixed", "base_link"); - stripLeadingSlash(this->fixedFrame, true); - this->sensorFrame = this->getParamVerbose("frames/sensor", ""); - stripLeadingSlash(this->sensorFrame, true); - this->filteringFrame = this->getParamVerbose("frames/filtering", this->fixedFrame); - stripLeadingSlash(this->filteringFrame, true); - this->minDistance = this->getParamVerbose("sensor/min_distance", 0.0, "m"); - this->maxDistance = this->getParamVerbose("sensor/max_distance", 0.0, "m"); - this->robotDescriptionParam = this->getParamVerbose("body_model/robot_description_param", "robot_description"); - this->keepCloudsOrganized = this->getParamVerbose("filter/keep_clouds_organized", true); - this->modelPoseUpdateInterval = this->getParamVerbose("filter/model_pose_update_interval", ros::Duration(0, 0), "s"); - const bool doClipping = this->getParamVerbose("filter/do_clipping", true); - const bool doContainsTest = this->getParamVerbose("filter/do_contains_test", true); - const bool doShadowTest = this->getParamVerbose("filter/do_shadow_test", true); - const double maxShadowDistance = this->getParamVerbose("filter/max_shadow_distance", this->maxDistance, "m"); - this->reachableTransformTimeout = this->getParamVerbose("transforms/timeout/reachable", ros::Duration(0.1), "s"); - this->unreachableTransformTimeout = this->getParamVerbose("transforms/timeout/unreachable", ros::Duration(0.2), "s"); - this->requireAllFramesReachable = this->getParamVerbose("transforms/require_all_reachable", false); - this->publishNoBoundingSpherePointcloud = this->getParamVerbose("bounding_sphere/publish_cut_out_pointcloud", false); - this->publishNoBoundingBoxPointcloud = this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", false); - this->publishNoOrientedBoundingBoxPointcloud = this->getParamVerbose("oriented_bounding_box/publish_cut_out_pointcloud", false); - this->publishNoLocalBoundingBoxPointcloud = this->getParamVerbose("local_bounding_box/publish_cut_out_pointcloud", false); - this->computeBoundingSphere = this->getParamVerbose("bounding_sphere/compute", false) || this->publishNoBoundingSpherePointcloud; - this->computeBoundingBox = this->getParamVerbose("bounding_box/compute", false) || this->publishNoBoundingBoxPointcloud; - this->computeOrientedBoundingBox = this->getParamVerbose("oriented_bounding_box/compute", false) || this->publishNoOrientedBoundingBoxPointcloud; - this->computeLocalBoundingBox = this->getParamVerbose("local_bounding_box/compute", false) || this->publishNoLocalBoundingBoxPointcloud; - this->computeDebugBoundingSphere = this->getParamVerbose("bounding_sphere/debug", false); - this->computeDebugBoundingBox = this->getParamVerbose("bounding_box/debug", false); - this->computeDebugOrientedBoundingBox = this->getParamVerbose("oriented_bounding_box/debug", false); - this->computeDebugLocalBoundingBox = this->getParamVerbose("local_bounding_box/debug", false); - this->publishBoundingSphereMarker = this->getParamVerbose("bounding_sphere/marker", false); - this->publishBoundingBoxMarker = this->getParamVerbose("bounding_box/marker", false); - this->publishOrientedBoundingBoxMarker = this->getParamVerbose("oriented_bounding_box/marker", false); - this->publishLocalBoundingBoxMarker = this->getParamVerbose("local_bounding_box/marker", false); - this->localBoundingBoxFrame = this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); - this->publishDebugPclInside = this->getParamVerbose("debug/pcl/inside", false); - this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", false); - this->publishDebugPclShadow = this->getParamVerbose("debug/pcl/shadow", false); - this->publishDebugContainsMarker = this->getParamVerbose("debug/marker/contains", false); - this->publishDebugShadowMarker = this->getParamVerbose("debug/marker/shadow", false); - this->publishDebugBsphereMarker = this->getParamVerbose("debug/marker/bounding_sphere", false); - this->publishDebugBboxMarker = this->getParamVerbose("debug/marker/bounding_box", false); - - const auto inflationPadding = this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); - const auto inflationScale = this->getParamVerbose("body_model/inflation/scale", 1.0); - this->defaultContainsInflation.padding = this->getParamVerbose("body_model/inflation/contains_test/padding", inflationPadding, "m"); - this->defaultContainsInflation.scale = this->getParamVerbose("body_model/inflation/contains_test/scale", inflationScale); - this->defaultShadowInflation.padding = this->getParamVerbose("body_model/inflation/shadow_test/padding", inflationPadding, "m"); - this->defaultShadowInflation.scale = this->getParamVerbose("body_model/inflation/shadow_test/scale", inflationScale); - this->defaultBsphereInflation.padding = this->getParamVerbose("body_model/inflation/bounding_sphere/padding", inflationPadding, "m"); - this->defaultBsphereInflation.scale = this->getParamVerbose("body_model/inflation/bounding_sphere/scale", inflationScale); - this->defaultBboxInflation.padding = this->getParamVerbose("body_model/inflation/bounding_box/padding", inflationPadding, "m"); - this->defaultBboxInflation.scale = this->getParamVerbose("body_model/inflation/bounding_box/scale", inflationScale); + // this->fixedFrame = this->getParamVerbose("frames/fixed", "base_link"); + // stripLeadingSlash(this->fixedFrame, true); + // this->sensorFrame = this->getParamVerbose("frames/sensor", ""); + // stripLeadingSlash(this->sensorFrame, true); + // this->filteringFrame = this->getParamVerbose("frames/filtering", this->fixedFrame); + // stripLeadingSlash(this->filteringFrame, true); + // this->minDistance = this->getParamVerbose("sensor/min_distance", 0.0, "m"); + // this->maxDistance = this->getParamVerbose("sensor/max_distance", 0.0, "m"); + // this->robotDescriptionParam = this->getParamVerbose("body_model/robot_description_param", "robot_description"); + // this->keepCloudsOrganized = this->getParamVerbose("filter/keep_clouds_organized", true); + // this->modelPoseUpdateInterval = this->getParamVerbose("filter/model_pose_update_interval", ros::Duration(0, 0), "s"); + // const bool doClipping = this->getParamVerbose("filter/do_clipping", true); + // const bool doContainsTest = this->getParamVerbose("filter/do_contains_test", true); + // const bool doShadowTest = this->getParamVerbose("filter/do_shadow_test", true); + // const double maxShadowDistance = this->getParamVerbose("filter/max_shadow_distance", this->maxDistance, "m"); + // this->reachableTransformTimeout = this->getParamVerbose("transforms/timeout/reachable", ros::Duration(0.1), "s"); + // this->unreachableTransformTimeout = this->getParamVerbose("transforms/timeout/unreachable", ros::Duration(0.2), "s"); + // this->requireAllFramesReachable = this->getParamVerbose("transforms/require_all_reachable", false); + // this->publishNoBoundingSpherePointcloud = this->getParamVerbose("bounding_sphere/publish_cut_out_pointcloud", false); + // this->publishNoBoundingBoxPointcloud = this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", false); + // this->publishNoOrientedBoundingBoxPointcloud = this->getParamVerbose("oriented_bounding_box/publish_cut_out_pointcloud", false); + // this->publishNoLocalBoundingBoxPointcloud = this->getParamVerbose("local_bounding_box/publish_cut_out_pointcloud", false); + // this->computeBoundingSphere = this->getParamVerbose("bounding_sphere/compute", false) || this->publishNoBoundingSpherePointcloud; + // this->computeBoundingBox = this->getParamVerbose("bounding_box/compute", false) || this->publishNoBoundingBoxPointcloud; + // this->computeOrientedBoundingBox = this->getParamVerbose("oriented_bounding_box/compute", false) || this->publishNoOrientedBoundingBoxPointcloud; + // this->computeLocalBoundingBox = this->getParamVerbose("local_bounding_box/compute", false) || this->publishNoLocalBoundingBoxPointcloud; + // this->computeDebugBoundingSphere = this->getParamVerbose("bounding_sphere/debug", false); + // this->computeDebugBoundingBox = this->getParamVerbose("bounding_box/debug", false); + // this->computeDebugOrientedBoundingBox = this->getParamVerbose("oriented_bounding_box/debug", false); + // this->computeDebugLocalBoundingBox = this->getParamVerbose("local_bounding_box/debug", false); + // this->publishBoundingSphereMarker = this->getParamVerbose("bounding_sphere/marker", false); + // this->publishBoundingBoxMarker = this->getParamVerbose("bounding_box/marker", false); + // this->publishOrientedBoundingBoxMarker = this->getParamVerbose("oriented_bounding_box/marker", false); + // this->publishLocalBoundingBoxMarker = this->getParamVerbose("local_bounding_box/marker", false); + // this->localBoundingBoxFrame = this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); + // this->publishDebugPclInside = this->getParamVerbose("debug/pcl/inside", false); + // this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", false); + // this->publishDebugPclShadow = this->getParamVerbose("debug/pcl/shadow", false); + // this->publishDebugContainsMarker = this->getParamVerbose("debug/marker/contains", false); + // this->publishDebugShadowMarker = this->getParamVerbose("debug/marker/shadow", false); + // this->publishDebugBsphereMarker = this->getParamVerbose("debug/marker/bounding_sphere", false); + // this->publishDebugBboxMarker = this->getParamVerbose("debug/marker/bounding_box", false); + + // const auto inflationPadding = this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); + // const auto inflationScale = this->getParamVerbose("body_model/inflation/scale", 1.0); + // this->defaultContainsInflation.padding = this->getParamVerbose("body_model/inflation/contains_test/padding", inflationPadding, "m"); + // this->defaultContainsInflation.scale = this->getParamVerbose("body_model/inflation/contains_test/scale", inflationScale); + // this->defaultShadowInflation.padding = this->getParamVerbose("body_model/inflation/shadow_test/padding", inflationPadding, "m"); + // this->defaultShadowInflation.scale = this->getParamVerbose("body_model/inflation/shadow_test/scale", inflationScale); + // this->defaultBsphereInflation.padding = this->getParamVerbose("body_model/inflation/bounding_sphere/padding", inflationPadding, "m"); + // this->defaultBsphereInflation.scale = this->getParamVerbose("body_model/inflation/bounding_sphere/scale", inflationScale); + // this->defaultBboxInflation.padding = this->getParamVerbose("body_model/inflation/bounding_box/padding", inflationPadding, "m"); + // this->defaultBboxInflation.scale = this->getParamVerbose("body_model/inflation/bounding_box/scale", inflationScale); // read per-link padding - const auto perLinkInflationPadding = this->getParamVerboseMap("body_model/inflation/per_link/padding", std::map(), "m"); - for (const auto& inflationPair : perLinkInflationPadding) - { - bool containsOnly; - bool shadowOnly; - bool bsphereOnly; - bool bboxOnly; - - auto linkName = inflationPair.first; - linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); - linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); - linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); - linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); - - if (!shadowOnly && !bsphereOnly && !bboxOnly) - this->perLinkContainsInflation[linkName] = - ScaleAndPadding(this->defaultContainsInflation.scale, inflationPair.second); - if (!containsOnly && !bsphereOnly && !bboxOnly) - this->perLinkShadowInflation[linkName] = - ScaleAndPadding(this->defaultShadowInflation.scale, inflationPair.second); - if (!containsOnly && !shadowOnly && !bboxOnly) - this->perLinkBsphereInflation[linkName] = - ScaleAndPadding(this->defaultBsphereInflation.scale, inflationPair.second); - if (!containsOnly && !shadowOnly && !bsphereOnly) - this->perLinkBboxInflation[linkName] = - ScaleAndPadding(this->defaultBboxInflation.scale, inflationPair.second); - } + // const auto perLinkInflationPadding = this->getParamVerboseMap("body_model/inflation/per_link/padding", std::map(), "m"); + // for (const auto& inflationPair : perLinkInflationPadding) + // { + // bool containsOnly; + // bool shadowOnly; + // bool bsphereOnly; + // bool bboxOnly; + + // auto linkName = inflationPair.first; + // linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); + // linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); + // linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); + // linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); + + // if (!shadowOnly && !bsphereOnly && !bboxOnly) + // this->perLinkContainsInflation[linkName] = + // ScaleAndPadding(this->defaultContainsInflation.scale, inflationPair.second); + // if (!containsOnly && !bsphereOnly && !bboxOnly) + // this->perLinkShadowInflation[linkName] = + // ScaleAndPadding(this->defaultShadowInflation.scale, inflationPair.second); + // if (!containsOnly && !shadowOnly && !bboxOnly) + // this->perLinkBsphereInflation[linkName] = + // ScaleAndPadding(this->defaultBsphereInflation.scale, inflationPair.second); + // if (!containsOnly && !shadowOnly && !bsphereOnly) + // this->perLinkBboxInflation[linkName] = + // ScaleAndPadding(this->defaultBboxInflation.scale, inflationPair.second); + // } // read per-link scale - const auto perLinkInflationScale = this->getParamVerboseMap("body_model/inflation/per_link/scale", std::map()); - for (const auto& inflationPair : perLinkInflationScale) - { - bool containsOnly; - bool shadowOnly; - bool bsphereOnly; - bool bboxOnly; - - auto linkName = inflationPair.first; - linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); - linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); - linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); - linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); - - if (!shadowOnly && !bsphereOnly && !bboxOnly) - { - if (this->perLinkContainsInflation.find(linkName) == this->perLinkContainsInflation.end()) - this->perLinkContainsInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultContainsInflation.padding); - else - this->perLinkContainsInflation[linkName].scale = inflationPair.second; - } - - if (!containsOnly && !bsphereOnly && !bboxOnly) - { - if (this->perLinkShadowInflation.find(linkName) == this->perLinkShadowInflation.end()) - this->perLinkShadowInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultShadowInflation.padding); - else - this->perLinkShadowInflation[linkName].scale = inflationPair.second; - } - - if (!containsOnly && !shadowOnly && !bboxOnly) - { - if (this->perLinkBsphereInflation.find(linkName) == this->perLinkBsphereInflation.end()) - this->perLinkBsphereInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBsphereInflation.padding); - else - this->perLinkBsphereInflation[linkName].scale = inflationPair.second; - } - - if (!containsOnly && !shadowOnly && !bsphereOnly) - { - if (this->perLinkBboxInflation.find(linkName) == this->perLinkBboxInflation.end()) - this->perLinkBboxInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBboxInflation.padding); - else - this->perLinkBboxInflation[linkName].scale = inflationPair.second; - } - } + // const auto perLinkInflationScale = this->getParamVerboseMap("body_model/inflation/per_link/scale", std::map()); + // for (const auto& inflationPair : perLinkInflationScale) + // { + // bool containsOnly; + // bool shadowOnly; + // bool bsphereOnly; + // bool bboxOnly; + + // auto linkName = inflationPair.first; + // linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); + // linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); + // linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); + // linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); + + // if (!shadowOnly && !bsphereOnly && !bboxOnly) + // { + // if (this->perLinkContainsInflation.find(linkName) == this->perLinkContainsInflation.end()) + // this->perLinkContainsInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultContainsInflation.padding); + // else + // this->perLinkContainsInflation[linkName].scale = inflationPair.second; + // } + + // if (!containsOnly && !bsphereOnly && !bboxOnly) + // { + // if (this->perLinkShadowInflation.find(linkName) == this->perLinkShadowInflation.end()) + // this->perLinkShadowInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultShadowInflation.padding); + // else + // this->perLinkShadowInflation[linkName].scale = inflationPair.second; + // } + + // if (!containsOnly && !shadowOnly && !bboxOnly) + // { + // if (this->perLinkBsphereInflation.find(linkName) == this->perLinkBsphereInflation.end()) + // this->perLinkBsphereInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBsphereInflation.padding); + // else + // this->perLinkBsphereInflation[linkName].scale = inflationPair.second; + // } + + // if (!containsOnly && !shadowOnly && !bsphereOnly) + // { + // if (this->perLinkBboxInflation.find(linkName) == this->perLinkBboxInflation.end()) + // this->perLinkBboxInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBboxInflation.padding); + // else + // this->perLinkBboxInflation[linkName].scale = inflationPair.second; + // } + // } // can contain either whole link names, or scoped names of their collisions (i.e. "link::collision_1" or "link::my_collision") - this->linksIgnoredInBoundingSphere = this->template getParamVerboseSet("ignored_links/bounding_sphere"); - this->linksIgnoredInBoundingBox = this->template getParamVerboseSet("ignored_links/bounding_box"); - this->linksIgnoredInContainsTest = this->template getParamVerboseSet("ignored_links/contains_test"); - this->linksIgnoredInShadowTest = this->template getParamVerboseSet("ignored_links/shadow_test", { "laser" }); - this->linksIgnoredEverywhere = this->template getParamVerboseSet("ignored_links/everywhere"); - this->onlyLinks = this->template getParamVerboseSet("only_links"); - - this->robotDescriptionUpdatesFieldName = this->getParamVerbose("body_model/dynamic_robot_description/field_name", "robot_model"); - // subscribe for robot_description param changes - this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( - "dynamic_robot_model_server/parameter_updates", 10, &RobotBodyFilter::robotDescriptionUpdated, this); - - this->reloadRobotModelServiceServer = this->privateNodeHandle.advertiseService( - "reload_model", &RobotBodyFilter::triggerModelReload, this); - - if (this->computeBoundingSphere) { - this->boundingSpherePublisher = this->nodeHandle.template advertise("robot_bounding_sphere", 100); - } - - if (this->computeBoundingBox) { - this->boundingBoxPublisher = this->nodeHandle.template advertise("robot_bounding_box", 100); - } - - if (this->computeOrientedBoundingBox) { - this->orientedBoundingBoxPublisher = this->nodeHandle.template advertise("robot_oriented_bounding_box", 100); - } - - if (this->computeLocalBoundingBox) { - this->localBoundingBoxPublisher = this->nodeHandle.template advertise("robot_local_bounding_box", 100); - } - - if (this->publishBoundingSphereMarker && this->computeBoundingSphere) { - this->boundingSphereMarkerPublisher = this->nodeHandle.template advertise("robot_bounding_sphere_marker", 100); - } - - if (this->publishBoundingBoxMarker && this->computeBoundingBox) { - this->boundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_bounding_box_marker", 100); - } - - if (this->publishOrientedBoundingBoxMarker && this->computeOrientedBoundingBox) { - this->orientedBoundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_oriented_bounding_box_marker", 100); - } - - if (this->publishLocalBoundingBoxMarker && this->computeLocalBoundingBox) { - this->localBoundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_local_bounding_box_marker", 100); - } - - if (this->publishNoBoundingBoxPointcloud) - { - this->scanPointCloudNoBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_bbox", 100); - } - - if (this->publishNoOrientedBoundingBoxPointcloud) - { - this->scanPointCloudNoOrientedBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_oriented_bbox", 100); - } - - if (this->publishNoLocalBoundingBoxPointcloud) - { - this->scanPointCloudNoLocalBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_local_bbox", 100); - } - - if (this->publishNoBoundingSpherePointcloud) - { - this->scanPointCloudNoBoundingSpherePublisher = this->nodeHandle.template advertise("scan_point_cloud_no_bsphere", 100); - } - - if (this->publishDebugPclInside) - { - this->debugPointCloudInsidePublisher = this->nodeHandle.template advertise("scan_point_cloud_inside", 100); - } - - if (this->publishDebugPclClip) - { - this->debugPointCloudClipPublisher = this->nodeHandle.template advertise("scan_point_cloud_clip", 100); - } - - if (this->publishDebugPclShadow) - { - this->debugPointCloudShadowPublisher = this->nodeHandle.template advertise("scan_point_cloud_shadow", 100); - } - - if (this->publishDebugContainsMarker) - { - this->debugContainsMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_contains_test", 100); - } - - if (this->publishDebugShadowMarker) - { - this->debugShadowMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_shadow_test", 100); - } - - if (this->publishDebugBsphereMarker) - { - this->debugBsphereMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_bounding_sphere", 100); - } - - if (this->publishDebugBboxMarker) - { - this->debugBboxMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_bounding_box", 100); - } - - if (this->computeDebugBoundingBox) { - this->boundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( - "robot_bounding_box_debug", 100); - } - - if (this->computeDebugOrientedBoundingBox) { - this->orientedBoundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( - "robot_oriented_bounding_box_debug", 100); - } - - if (this->computeDebugLocalBoundingBox) { - this->localBoundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( - "robot_local_bounding_box_debug", 100); - } - - if (this->computeDebugBoundingSphere) { - this->boundingSphereDebugMarkerPublisher = this->nodeHandle.template advertise( - "robot_bounding_sphere_debug", 100); - } + // this->linksIgnoredInBoundingSphere = this->template getParamVerboseSet("ignored_links/bounding_sphere"); + // this->linksIgnoredInBoundingBox = this->template getParamVerboseSet("ignored_links/bounding_box"); + // this->linksIgnoredInContainsTest = this->template getParamVerboseSet("ignored_links/contains_test"); + // this->linksIgnoredInShadowTest = this->template getParamVerboseSet("ignored_links/shadow_test", { "laser" }); + // this->linksIgnoredEverywhere = this->template getParamVerboseSet("ignored_links/everywhere"); + // this->onlyLinks = this->template getParamVerboseSet("only_links"); + + // this->robotDescriptionUpdatesFieldName = this->getParamVerbose("body_model/dynamic_robot_description/field_name", "robot_model"); + // // subscribe for robot_description param changes + // this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( + // "dynamic_robot_model_server/parameter_updates", 10, &RobotBodyFilter::robotDescriptionUpdated, this); + + // this->reloadRobotModelServiceServer = this->privateNodeHandle.advertiseService( + // "reload_model", &RobotBodyFilter::triggerModelReload, this); + + // if (this->computeBoundingSphere) { + // this->boundingSpherePublisher = this->nodeHandle.template advertise("robot_bounding_sphere", 100); + // } + + // if (this->computeBoundingBox) { + // this->boundingBoxPublisher = this->nodeHandle.template advertise("robot_bounding_box", 100); + // } + + // if (this->computeOrientedBoundingBox) { + // this->orientedBoundingBoxPublisher = this->nodeHandle.template advertise("robot_oriented_bounding_box", 100); + // } + + // if (this->computeLocalBoundingBox) { + // this->localBoundingBoxPublisher = this->nodeHandle.template advertise("robot_local_bounding_box", 100); + // } + + // if (this->publishBoundingSphereMarker && this->computeBoundingSphere) { + // this->boundingSphereMarkerPublisher = this->nodeHandle.template advertise("robot_bounding_sphere_marker", 100); + // } + + // if (this->publishBoundingBoxMarker && this->computeBoundingBox) { + // this->boundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_bounding_box_marker", 100); + // } + + // if (this->publishOrientedBoundingBoxMarker && this->computeOrientedBoundingBox) { + // this->orientedBoundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_oriented_bounding_box_marker", 100); + // } + + // if (this->publishLocalBoundingBoxMarker && this->computeLocalBoundingBox) { + // this->localBoundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_local_bounding_box_marker", 100); + // } + + // if (this->publishNoBoundingBoxPointcloud) + // { + // this->scanPointCloudNoBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_bbox", 100); + // } + + // if (this->publishNoOrientedBoundingBoxPointcloud) + // { + // this->scanPointCloudNoOrientedBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_oriented_bbox", 100); + // } + + // if (this->publishNoLocalBoundingBoxPointcloud) + // { + // this->scanPointCloudNoLocalBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_local_bbox", 100); + // } + + // if (this->publishNoBoundingSpherePointcloud) + // { + // this->scanPointCloudNoBoundingSpherePublisher = this->nodeHandle.template advertise("scan_point_cloud_no_bsphere", 100); + // } + + // if (this->publishDebugPclInside) + // { + // this->debugPointCloudInsidePublisher = this->nodeHandle.template advertise("scan_point_cloud_inside", 100); + // } + + // if (this->publishDebugPclClip) + // { + // this->debugPointCloudClipPublisher = this->nodeHandle.template advertise("scan_point_cloud_clip", 100); + // } + + // if (this->publishDebugPclShadow) + // { + // this->debugPointCloudShadowPublisher = this->nodeHandle.template advertise("scan_point_cloud_shadow", 100); + // } + + // if (this->publishDebugContainsMarker) + // { + // this->debugContainsMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_contains_test", 100); + // } + + // if (this->publishDebugShadowMarker) + // { + // this->debugShadowMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_shadow_test", 100); + // } + + // if (this->publishDebugBsphereMarker) + // { + // this->debugBsphereMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_bounding_sphere", 100); + // } + + // if (this->publishDebugBboxMarker) + // { + // this->debugBboxMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_bounding_box", 100); + // } + + // if (this->computeDebugBoundingBox) { + // this->boundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( + // "robot_bounding_box_debug", 100); + // } + + // if (this->computeDebugOrientedBoundingBox) { + // this->orientedBoundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( + // "robot_oriented_bounding_box_debug", 100); + // } + + // if (this->computeDebugLocalBoundingBox) { + // this->localBoundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( + // "robot_local_bounding_box_debug", 100); + // } + + // if (this->computeDebugBoundingSphere) { + // this->boundingSphereDebugMarkerPublisher = this->nodeHandle.template advertise( + // "robot_bounding_sphere_debug", 100); + // } // initialize the 3D body masking tool - auto getShapeTransformCallback = std::bind(&RobotBodyFilter::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2); - shapeMask = std::make_unique(getShapeTransformCallback, - this->minDistance, this->maxDistance, - doClipping, doContainsTest, doShadowTest, maxShadowDistance); - - // the other case happens when configure() is called again from update() (e.g. when a new bag file - // started playing) - if (this->tfFramesWatchdog == nullptr) { - std::set initialMonitoredFrames; - if (!this->sensorFrame.empty()) - { - initialMonitoredFrames.insert(this->sensorFrame); - } - - this->tfFramesWatchdog = std::make_shared(this->filteringFrame, - initialMonitoredFrames, this->tfBuffer, - this->unreachableTransformTimeout, ros::Rate(ros::Duration(1.0))); - this->tfFramesWatchdog->start(); - } - - { // initialize the robot body to be masked out - - string robotUrdf; - while (!this->nodeHandle.getParam(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { - if (this->failWithoutRobotDescription) - { - throw std::runtime_error( - "RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); - } - if (!ros::ok()) - return false; - - ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide the robot model. Waiting 1s.", - robotDescriptionParam.c_str()); - ros::Duration(1.0).sleep(); - } - - // happens when configure() is called again from update() (e.g. when a new bag file started - // playing) - if (!this->shapesToLinks.empty()) - this->clearRobotMask(); - this->addRobotMaskFromUrdf(robotUrdf); - } - - ROS_INFO("RobotBodyFilter: Successfully configured."); - ROS_INFO("Filtering data in frame %s", this->filteringFrame.c_str()); - ROS_INFO("RobotBodyFilter: Filtering into the following categories:"); - ROS_INFO("RobotBodyFilter: \tOUTSIDE"); - if (doClipping) ROS_INFO("RobotBodyFilter: \tCLIP"); - if (doContainsTest) ROS_INFO("RobotBodyFilter: \tINSIDE"); - if (doShadowTest) ROS_INFO("RobotBodyFilter: \tSHADOW"); - - if (this->onlyLinks.empty()) { - if (this->linksIgnoredEverywhere.empty()) { - ROS_INFO("RobotBodyFilter: Filtering applied to all links."); - } else { - ROS_INFO("RobotBodyFilter: Filtering applied to all links except %s.", to_string(this->linksIgnoredEverywhere).c_str()); - } - } else { - if (this->linksIgnoredEverywhere.empty()) { - ROS_INFO("RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); - } else { - ROS_INFO("RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", to_string(this->onlyLinks).c_str(), to_string(this->linksIgnoredEverywhere).c_str()); - } - } - - this->timeConfigured = ros::Time::now(); + // auto getShapeTransformCallback = std::bind(&RobotBodyFilter::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2); + // shapeMask = std::make_unique(getShapeTransformCallback, + // this->minDistance, this->maxDistance, + // doClipping, doContainsTest, doShadowTest, maxShadowDistance); + + // // the other case happens when configure() is called again from update() (e.g. when a new bag file + // // started playing) + // if (this->tfFramesWatchdog == nullptr) { + // std::set initialMonitoredFrames; + // if (!this->sensorFrame.empty()) + // { + // initialMonitoredFrames.insert(this->sensorFrame); + // } + + // this->tfFramesWatchdog = std::make_shared(this->filteringFrame, + // initialMonitoredFrames, this->tfBuffer, + // this->unreachableTransformTimeout, ros::Rate(ros::Duration(1.0))); + // this->tfFramesWatchdog->start(); + // } + + // { // initialize the robot body to be masked out + + // string robotUrdf; + // while (!this->nodeHandle.getParam(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { + // if (this->failWithoutRobotDescription) + // { + // throw std::runtime_error( + // "RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); + // } + // if (!ros::ok()) + // return false; + + // ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide the robot model. Waiting 1s.", + // robotDescriptionParam.c_str()); + // ros::Duration(1.0).sleep(); + // } + + // // happens when configure() is called again from update() (e.g. when a new bag file started + // // playing) + // if (!this->shapesToLinks.empty()) + // this->clearRobotMask(); + // this->addRobotMaskFromUrdf(robotUrdf); + // } + + // ROS_INFO("RobotBodyFilter: Successfully configured."); + // ROS_INFO("Filtering data in frame %s", this->filteringFrame.c_str()); + // ROS_INFO("RobotBodyFilter: Filtering into the following categories:"); + // ROS_INFO("RobotBodyFilter: \tOUTSIDE"); + // if (doClipping) ROS_INFO("RobotBodyFilter: \tCLIP"); + // if (doContainsTest) ROS_INFO("RobotBodyFilter: \tINSIDE"); + // if (doShadowTest) ROS_INFO("RobotBodyFilter: \tSHADOW"); + + // if (this->onlyLinks.empty()) { + // if (this->linksIgnoredEverywhere.empty()) { + // ROS_INFO("RobotBodyFilter: Filtering applied to all links."); + // } else { + // ROS_INFO("RobotBodyFilter: Filtering applied to all links except %s.", to_string(this->linksIgnoredEverywhere).c_str()); + // } + // } else { + // if (this->linksIgnoredEverywhere.empty()) { + // ROS_INFO("RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); + // } else { + // ROS_INFO("RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", to_string(this->onlyLinks).c_str(), to_string(this->linksIgnoredEverywhere).c_str()); + // } + // } + + // this->timeConfigured = ros::Time::now(); return true; } bool RobotBodyFilterLaserScan::configure() { - this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", true); + // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", true); - bool success = RobotBodyFilter::configure(); - return success; + // bool success = RobotBodyFilter::configure(); + return false; } bool RobotBodyFilterPointCloud2::configure() { - this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", false); + // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", false); - bool success = RobotBodyFilter::configure(); - if (!success) - return false; + // bool success = RobotBodyFilter::configure(); + // if (!success) + // return false; - this->outputFrame = this->getParamVerbose("frames/output", this->filteringFrame); + // this->outputFrame = this->getParamVerbose("frames/output", this->filteringFrame); - const auto pointChannels = this->getParamVerbose("cloud/point_channels", std::vector{"vp_"}); - const auto directionChannels = this->getParamVerbose("cloud/direction_channels", std::vector{"normal_"}); + // const auto pointChannels = this->getParamVerbose("cloud/point_channels", std::vector{"vp_"}); + // const auto directionChannels = this->getParamVerbose("cloud/direction_channels", std::vector{"normal_"}); - for (const auto& channel : pointChannels) - this->channelsToTransform[channel] = CloudChannelType::POINT; - for (const auto& channel : directionChannels) - this->channelsToTransform[channel] = CloudChannelType::DIRECTION; + // for (const auto& channel : pointChannels) + // this->channelsToTransform[channel] = CloudChannelType::POINT; + // for (const auto& channel : directionChannels) + // this->channelsToTransform[channel] = CloudChannelType::DIRECTION; - stripLeadingSlash(this->outputFrame, true); + // stripLeadingSlash(this->outputFrame, true); return true; } template bool RobotBodyFilter::computeMask( - const sensor_msgs::PointCloud2 &projectedPointCloud, + const sensor_msgs::msg::PointCloud2 &projectedPointCloud, std::vector &pointMask, const std::string &sensorFrame) { @@ -432,7 +432,7 @@ bool RobotBodyFilter::computeMask( remainingTime(scanTime, this->reachableTransformTimeout)); tf2::fromMsg(sensorTf.transform.translation, sensorPosition); } catch (tf2::TransformException& e) { - ROS_ERROR("RobotBodyFilter: Could not compute filtering mask due to this " + RCLCPP_ERROR(nodeHandle.get_logger(), "RobotBodyFilter: Could not compute filtering mask due to this " "TF exception: %s", e.what()); return false; } @@ -460,7 +460,7 @@ bool RobotBodyFilter::computeMask( if ((*stamps_end_it) > static_cast(scanDuration)) scanDuration = static_cast(*stamps_end_it); } - const ros::Time afterScanTime(scanTime + ros::Duration().fromSec(scanDuration)); + const rclcpp::Time afterScanTime(scanTime + rclcpp::Duration::from_seconds(scanDuration)); size_t updateBodyPosesEvery; if (this->modelPoseUpdateInterval.sec == 0 && this->modelPoseUpdateInterval.nsec == 0) { @@ -476,7 +476,7 @@ bool RobotBodyFilter::computeMask( // isn't really taken point by point with different timestamps if (scanDuration == 0.0) { updateBodyPosesEvery = num_points(projectedPointCloud) + 1; - ROS_WARN_ONCE("RobotBodyFilter: sensor/point_by_point is set to true but " + RCLCPP_WARN_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: sensor/point_by_point is set to true but " "all points in the cloud have the same timestamp. You should" " change the parameter to false to gain performance."); } @@ -512,7 +512,7 @@ bool RobotBodyFilter::computeMask( } } - ROS_DEBUG("RobotBodyFilter: Mask computed in %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); + RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); this->publishDebugPointClouds(projectedPointCloud, pointMask); this->publishDebugMarkers(scanTime); @@ -521,7 +521,7 @@ bool RobotBodyFilter::computeMask( this->computeAndPublishOrientedBoundingBox(projectedPointCloud); this->computeAndPublishLocalBoundingBox(projectedPointCloud); - ROS_DEBUG("RobotBodyFilter: Filtering run time is %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); + RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); return true; } From cb370d83dd4d259ffd829bf98389aa46c7442dad Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 16 Apr 2024 15:34:44 -0400 Subject: [PATCH 05/54] in progress --- include/robot_body_filter/RobotBodyFilter.h | 121 +- include/robot_body_filter/utils/tf2_eigen.h | 6 +- .../robot_body_filter/utils/tf2_sensor_msgs.h | 26 +- .../robot_body_filter/utils/time_utils.hpp | 6 +- src/RobotBodyFilter.cpp | 1582 ++++++++++------- src/utils/tf2_eigen.cpp | 2 +- src/utils/tf2_sensor_msgs.cpp | 28 +- 7 files changed, 1053 insertions(+), 718 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 5eef8b4..8a3e01b 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -1,9 +1,12 @@ #ifndef ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ #define ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ +#include "robot_body_filter/utils/tf2_sensor_msgs.h" #include #include +#include #include +#include #include #include #include @@ -37,6 +40,7 @@ #include #include #include +#include #include #include @@ -97,8 +101,7 @@ static const std::string BBOX_SUFFIX = "::bounding_box"; * * \author Martin Pecka */ -template -class RobotBodyFilter : public filters::FilterBase { +template class RobotBodyFilter : public filters::FilterBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -111,7 +114,7 @@ class RobotBodyFilter : public filters::FilterBase { bool update(const T &data_in, T &data_out) override = 0; -protected: + // protected: //! Handle of the node this filter runs in. rclcpp::Node nodeHandle; rclcpp::Node privateNodeHandle; @@ -231,46 +234,72 @@ class RobotBodyFilter : public filters::FilterBase { //! Publisher of robot bounding sphere (relative to fixed frame). // ros::Publisher boundingSpherePublisher; + // They use a custom msg type, we can just use an unstamped std shape_msg for + // simplicity + rclcpp::Publisher::SharedPtr + boundingSpherePublisher; // //! Publisher of robot bounding box (relative to fixed frame). - // ros::Publisher boundingBoxPublisher; + rclcpp::Publisher::SharedPtr + boundingBoxPublisher; // //! Publisher of robot bounding box (relative to fixed frame). - // ros::Publisher orientedBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr + orientedBoundingBoxPublisher; // //! Publisher of robot bounding box (relative to defined local frame). - // ros::Publisher localBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr + localBoundingBoxPublisher; // //! Publisher of the bounding sphere marker. - // ros::Publisher boundingSphereMarkerPublisher; + rclcpp::Publisher::SharedPtr + boundingSphereMarkerPublisher; // //! Publisher of the bounding box marker. - // ros::Publisher boundingBoxMarkerPublisher; + rclcpp::Publisher::SharedPtr + boundingBoxMarkerPublisher; // //! Publisher of the oriented bounding box marker. - // ros::Publisher orientedBoundingBoxMarkerPublisher; - // //! Publisher of the local bounding box marker. - // ros::Publisher localBoundingBoxMarkerPublisher; - // //! Publisher of the debug bounding box markers. - // ros::Publisher boundingBoxDebugMarkerPublisher; - // //! Publisher of the debug oriented bounding box markers. - // ros::Publisher orientedBoundingBoxDebugMarkerPublisher; - // //! Publisher of the debug local bounding box markers. - // ros::Publisher localBoundingBoxDebugMarkerPublisher; - // //! Publisher of the debug bounding sphere markers. - // ros::Publisher boundingSphereDebugMarkerPublisher; - - // //! Publisher of scan_point_cloud with robot bounding box cut out. - // ros::Publisher scanPointCloudNoBoundingBoxPublisher; - // //! Publisher of scan_point_cloud with robot oriented bounding box cut out. - // ros::Publisher scanPointCloudNoOrientedBoundingBoxPublisher; - // //! Publisher of scan_point_cloud with robot local bounding box cut out. - // ros::Publisher scanPointCloudNoLocalBoundingBoxPublisher; - // //! Publisher of scan_point_cloud with robot bounding sphere cut out. - // ros::Publisher scanPointCloudNoBoundingSpherePublisher; - // ros::Publisher debugPointCloudInsidePublisher; - // ros::Publisher debugPointCloudClipPublisher; - // ros::Publisher debugPointCloudShadowPublisher; - // ros::Publisher debugContainsMarkerPublisher; - // ros::Publisher debugShadowMarkerPublisher; - // ros::Publisher debugBsphereMarkerPublisher; - // ros::Publisher debugBboxMarkerPublisher; - - //! Service server for reloading robot model. + rclcpp::Publisher::SharedPtr + orientedBoundingBoxMarkerPublisher; + //! Publisher of the local bounding box marker. + rclcpp::Publisher::SharedPtr + localBoundingBoxMarkerPublisher; + //! Publisher of the debug bounding box markers. + rclcpp::Publisher::SharedPtr + boundingBoxDebugMarkerPublisher; + //! Publisher of the debug oriented bounding box markers. + rclcpp::Publisher::SharedPtr + orientedBoundingBoxDebugMarkerPublisher; + //! Publisher of the debug local bounding box markers. + rclcpp::Publisher::SharedPtr + localBoundingBoxDebugMarkerPublisher; + //! Publisher of the debug bounding sphere markers. + rclcpp::Publisher::SharedPtr + boundingSphereDebugMarkerPublisher; + + //! Publisher of scan_point_cloud with robot bounding box cut out. + rclcpp::Publisher::SharedPtr + scanPointCloudNoBoundingBoxPublisher; + //! Publisher of scan_point_cloud with robot oriented bounding box cut out. + rclcpp::Publisher::SharedPtr + scanPointCloudNoOrientedBoundingBoxPublisher; + //! Publisher of scan_point_cloud with robot local bounding box cut out. + rclcpp::Publisher::SharedPtr + scanPointCloudNoLocalBoundingBoxPublisher; + //! Publisher of scan_point_cloud with robot bounding sphere cut out. + rclcpp::Publisher::SharedPtr + scanPointCloudNoBoundingSpherePublisher; + rclcpp::Publisher::SharedPtr + debugPointCloudInsidePublisher; + rclcpp::Publisher::SharedPtr + debugPointCloudClipPublisher; + rclcpp::Publisher::SharedPtr + debugPointCloudShadowPublisher; + rclcpp::Publisher::SharedPtr + debugContainsMarkerPublisher; + rclcpp::Publisher::SharedPtr + debugShadowMarkerPublisher; + rclcpp::Publisher::SharedPtr + debugBsphereMarkerPublisher; + rclcpp::Publisher::SharedPtr + debugBboxMarkerPublisher; + + // ! Service server for reloading robot model. // ros::ServiceServer reloadRobotModelServiceServer; //! Whether to compute bounding sphere of the robot. @@ -424,8 +453,9 @@ class RobotBodyFilter : public filters::FilterBase { * \param afterScantime The after scan time to get transforms for (if zero * time is passed, after scan transforms are not computed). */ - void updateTransformCache(const rclcpp::Time &time, - const rclcpp::Time &afterScanTime = rclcpp::Time(0)); + void + updateTransformCache(const rclcpp::Time &time, + const rclcpp::Time &afterScanTime = rclcpp::Time(0)); /** * \brief Callback handling update of the robot_description parameter using @@ -433,7 +463,8 @@ class RobotBodyFilter : public filters::FilterBase { * * \param newConfig The updated config. */ - // void robotDescriptionUpdated(dynamic_reconfigure::ConfigConstPtr newConfig); + // void robotDescriptionUpdated(dynamic_reconfigure::ConfigConstPtr + // newConfig); /** * \brief Callback for ~reload_model service. Reloads the URDF from parameter. @@ -505,7 +536,7 @@ class RobotBodyFilter : public filters::FilterBase { }; class RobotBodyFilterLaserScan - : public filters::FilterBase { + : public RobotBodyFilter { public: //! Apply the filter. bool update(const sensor_msgs::msg::LaserScan &inputScan, @@ -518,12 +549,12 @@ class RobotBodyFilterLaserScan // in RobotBodyFilterLaserScan::update we project the scan to a pointcloud // with viewpoints - // const std::unordered_map channelsToTransform{ - // {"vp_", CloudChannelType::POINT}}; + const std::unordered_map channelsToTransform{ + {"vp_", CloudChannelType::POINT}}; }; class RobotBodyFilterPointCloud2 - : public filters::FilterBase { + : public RobotBodyFilter { public: //! Apply the filter. bool update(const sensor_msgs::msg::PointCloud2 &inputCloud, @@ -535,7 +566,7 @@ class RobotBodyFilterPointCloud2 /** \brief Frame into which the output data should be transformed. */ std::string outputFrame; - // std::unordered_map channelsToTransform; + std::unordered_map channelsToTransform; }; } // namespace robot_body_filter diff --git a/include/robot_body_filter/utils/tf2_eigen.h b/include/robot_body_filter/utils/tf2_eigen.h index 62f5eb8..040abeb 100644 --- a/include/robot_body_filter/utils/tf2_eigen.h +++ b/include/robot_body_filter/utils/tf2_eigen.h @@ -2,10 +2,10 @@ #define ROBOT_BODY_FILTER_TF2_EIGEN_H #include -#include +#include namespace tf2 { -void toMsg(const Eigen::Vector3d& in, geometry_msgs::Point32& out); +void toMsg(const Eigen::Vector3d &in, geometry_msgs::msg::Point32 &out); } -#endif //ROBOT_BODY_FILTER_TF2_EIGEN_H +#endif // ROBOT_BODY_FILTER_TF2_EIGEN_H diff --git a/include/robot_body_filter/utils/tf2_sensor_msgs.h b/include/robot_body_filter/utils/tf2_sensor_msgs.h index 3997ff3..11865d2 100644 --- a/include/robot_body_filter/utils/tf2_sensor_msgs.h +++ b/include/robot_body_filter/utils/tf2_sensor_msgs.h @@ -10,31 +10,31 @@ namespace robot_body_filter enum class CloudChannelType { POINT, DIRECTION, SCALAR }; -void transformChannel(sensor_msgs::PointCloud2& cloud, const geometry_msgs::Transform& t, +void transformChannel(sensor_msgs::msg::PointCloud2& cloud, const geometry_msgs::msg::Transform& t, const std::string& channelPrefix, CloudChannelType type); -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf); +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf); -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf, +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf, const std::unordered_map& channels); -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, tf2_ros::Buffer& tfBuffer, const std::string& targetFrame); -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, tf2_ros::Buffer& tfBuffer, const std::string& targetFrame, const std::unordered_map& channels); -sensor_msgs::PointCloud2& transformOnlyChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf, +sensor_msgs::msg::PointCloud2& transformOnlyChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf, const std::unordered_map& channels); -sensor_msgs::PointCloud2& transformOnlyXYZ( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf); +sensor_msgs::msg::PointCloud2& transformOnlyXYZ( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf); } diff --git a/include/robot_body_filter/utils/time_utils.hpp b/include/robot_body_filter/utils/time_utils.hpp index ff6f748..278547d 100644 --- a/include/robot_body_filter/utils/time_utils.hpp +++ b/include/robot_body_filter/utils/time_utils.hpp @@ -13,7 +13,7 @@ namespace robot_body_filter { * @return The remaining time, or zero duration if the time is negative or ROS * time isn't initialized. */ -ros::Duration remainingTime(const ros::Time &query, double timeout); +rclcpp::Duration remainingTime(const rclcpp::Time &query, double timeout); /** * @brief remainingTime Return remaining time to timeout from the query time. @@ -22,8 +22,8 @@ ros::Duration remainingTime(const ros::Time &query, double timeout); * @return The remaining time, or zero duration if the time is negative or ROS * time isn't initialized. */ -ros::Duration remainingTime(const ros::Time &query, - const ros::Duration &timeout); +rclcpp::Duration remainingTime(const rclcpp::Time &query, + const rclcpp::Duration &timeout); }; // namespace robot_body_filter diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index f5af32b..6f3ff67 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1,5 +1,8 @@ +#include +#include #include +#include #include #include @@ -12,20 +15,20 @@ #include #include -#include #include +#include #include #include -#include #include +#include #include #include #include #include #include -// #include +#include // #include #include #include @@ -36,21 +39,22 @@ namespace robot_body_filter { -template +template RobotBodyFilter::RobotBodyFilter() : privateNodeHandle("~") { this->modelMutex.reset(new std::mutex()); } -template -bool RobotBodyFilter::configure() { - this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", rclcpp::Duration(60.0), "s"); +template bool RobotBodyFilter::configure() { + // this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", + // rclcpp::Duration(60.0), "s"); - if (this->tfBuffer == nullptr) - { + if (this->tfBuffer == nullptr) { this->tfBuffer = std::make_shared(this->tfBufferLength); - this->tfListener = std::make_unique(*this->tfBuffer); + this->tfListener = + std::make_unique(*this->tfBuffer); } else { - // clear the TF buffer (useful if calling configure() after receiving old TF data) + // clear the TF buffer (useful if calling configure() after receiving old TF + // data) this->tfBuffer->clear(); } @@ -58,59 +62,100 @@ bool RobotBodyFilter::configure() { // stripLeadingSlash(this->fixedFrame, true); // this->sensorFrame = this->getParamVerbose("frames/sensor", ""); // stripLeadingSlash(this->sensorFrame, true); - // this->filteringFrame = this->getParamVerbose("frames/filtering", this->fixedFrame); - // stripLeadingSlash(this->filteringFrame, true); + // this->filteringFrame = this->getParamVerbose("frames/filtering", + // this->fixedFrame); stripLeadingSlash(this->filteringFrame, true); // this->minDistance = this->getParamVerbose("sensor/min_distance", 0.0, "m"); // this->maxDistance = this->getParamVerbose("sensor/max_distance", 0.0, "m"); - // this->robotDescriptionParam = this->getParamVerbose("body_model/robot_description_param", "robot_description"); - // this->keepCloudsOrganized = this->getParamVerbose("filter/keep_clouds_organized", true); - // this->modelPoseUpdateInterval = this->getParamVerbose("filter/model_pose_update_interval", ros::Duration(0, 0), "s"); - // const bool doClipping = this->getParamVerbose("filter/do_clipping", true); - // const bool doContainsTest = this->getParamVerbose("filter/do_contains_test", true); - // const bool doShadowTest = this->getParamVerbose("filter/do_shadow_test", true); - // const double maxShadowDistance = this->getParamVerbose("filter/max_shadow_distance", this->maxDistance, "m"); - // this->reachableTransformTimeout = this->getParamVerbose("transforms/timeout/reachable", ros::Duration(0.1), "s"); - // this->unreachableTransformTimeout = this->getParamVerbose("transforms/timeout/unreachable", ros::Duration(0.2), "s"); - // this->requireAllFramesReachable = this->getParamVerbose("transforms/require_all_reachable", false); - // this->publishNoBoundingSpherePointcloud = this->getParamVerbose("bounding_sphere/publish_cut_out_pointcloud", false); - // this->publishNoBoundingBoxPointcloud = this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", false); - // this->publishNoOrientedBoundingBoxPointcloud = this->getParamVerbose("oriented_bounding_box/publish_cut_out_pointcloud", false); - // this->publishNoLocalBoundingBoxPointcloud = this->getParamVerbose("local_bounding_box/publish_cut_out_pointcloud", false); - // this->computeBoundingSphere = this->getParamVerbose("bounding_sphere/compute", false) || this->publishNoBoundingSpherePointcloud; - // this->computeBoundingBox = this->getParamVerbose("bounding_box/compute", false) || this->publishNoBoundingBoxPointcloud; - // this->computeOrientedBoundingBox = this->getParamVerbose("oriented_bounding_box/compute", false) || this->publishNoOrientedBoundingBoxPointcloud; - // this->computeLocalBoundingBox = this->getParamVerbose("local_bounding_box/compute", false) || this->publishNoLocalBoundingBoxPointcloud; - // this->computeDebugBoundingSphere = this->getParamVerbose("bounding_sphere/debug", false); - // this->computeDebugBoundingBox = this->getParamVerbose("bounding_box/debug", false); - // this->computeDebugOrientedBoundingBox = this->getParamVerbose("oriented_bounding_box/debug", false); - // this->computeDebugLocalBoundingBox = this->getParamVerbose("local_bounding_box/debug", false); - // this->publishBoundingSphereMarker = this->getParamVerbose("bounding_sphere/marker", false); - // this->publishBoundingBoxMarker = this->getParamVerbose("bounding_box/marker", false); - // this->publishOrientedBoundingBoxMarker = this->getParamVerbose("oriented_bounding_box/marker", false); - // this->publishLocalBoundingBoxMarker = this->getParamVerbose("local_bounding_box/marker", false); - // this->localBoundingBoxFrame = this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); - // this->publishDebugPclInside = this->getParamVerbose("debug/pcl/inside", false); - // this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", false); - // this->publishDebugPclShadow = this->getParamVerbose("debug/pcl/shadow", false); - // this->publishDebugContainsMarker = this->getParamVerbose("debug/marker/contains", false); - // this->publishDebugShadowMarker = this->getParamVerbose("debug/marker/shadow", false); - // this->publishDebugBsphereMarker = this->getParamVerbose("debug/marker/bounding_sphere", false); - // this->publishDebugBboxMarker = this->getParamVerbose("debug/marker/bounding_box", false); - - // const auto inflationPadding = this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); - // const auto inflationScale = this->getParamVerbose("body_model/inflation/scale", 1.0); - // this->defaultContainsInflation.padding = this->getParamVerbose("body_model/inflation/contains_test/padding", inflationPadding, "m"); - // this->defaultContainsInflation.scale = this->getParamVerbose("body_model/inflation/contains_test/scale", inflationScale); - // this->defaultShadowInflation.padding = this->getParamVerbose("body_model/inflation/shadow_test/padding", inflationPadding, "m"); - // this->defaultShadowInflation.scale = this->getParamVerbose("body_model/inflation/shadow_test/scale", inflationScale); - // this->defaultBsphereInflation.padding = this->getParamVerbose("body_model/inflation/bounding_sphere/padding", inflationPadding, "m"); - // this->defaultBsphereInflation.scale = this->getParamVerbose("body_model/inflation/bounding_sphere/scale", inflationScale); - // this->defaultBboxInflation.padding = this->getParamVerbose("body_model/inflation/bounding_box/padding", inflationPadding, "m"); - // this->defaultBboxInflation.scale = this->getParamVerbose("body_model/inflation/bounding_box/scale", inflationScale); + // this->robotDescriptionParam = + // this->getParamVerbose("body_model/robot_description_param", + // "robot_description"); this->keepCloudsOrganized = + // this->getParamVerbose("filter/keep_clouds_organized", true); + // this->modelPoseUpdateInterval = + // this->getParamVerbose("filter/model_pose_update_interval", + // rclcpp::Duration(0, 0), "s"); const bool doClipping = + // this->getParamVerbose("filter/do_clipping", true); const bool + // doContainsTest = this->getParamVerbose("filter/do_contains_test", true); + // const bool doShadowTest = this->getParamVerbose("filter/do_shadow_test", + // true); const double maxShadowDistance = + // this->getParamVerbose("filter/max_shadow_distance", this->maxDistance, + // "m"); this->reachableTransformTimeout = + // this->getParamVerbose("transforms/timeout/reachable", + // rclcpp::Duration(0.1), "s"); this->unreachableTransformTimeout = + // this->getParamVerbose("transforms/timeout/unreachable", + // rclcpp::Duration(0.2), "s"); this->requireAllFramesReachable = + // this->getParamVerbose("transforms/require_all_reachable", false); + // this->publishNoBoundingSpherePointcloud = + // this->getParamVerbose("bounding_sphere/publish_cut_out_pointcloud", false); + // this->publishNoBoundingBoxPointcloud = + // this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", false); + // this->publishNoOrientedBoundingBoxPointcloud = + // this->getParamVerbose("oriented_bounding_box/publish_cut_out_pointcloud", + // false); this->publishNoLocalBoundingBoxPointcloud = + // this->getParamVerbose("local_bounding_box/publish_cut_out_pointcloud", + // false); this->computeBoundingSphere = + // this->getParamVerbose("bounding_sphere/compute", false) || + // this->publishNoBoundingSpherePointcloud; this->computeBoundingBox = + // this->getParamVerbose("bounding_box/compute", false) || + // this->publishNoBoundingBoxPointcloud; this->computeOrientedBoundingBox = + // this->getParamVerbose("oriented_bounding_box/compute", false) || + // this->publishNoOrientedBoundingBoxPointcloud; this->computeLocalBoundingBox + // = this->getParamVerbose("local_bounding_box/compute", false) || + // this->publishNoLocalBoundingBoxPointcloud; this->computeDebugBoundingSphere + // = this->getParamVerbose("bounding_sphere/debug", false); + // this->computeDebugBoundingBox = this->getParamVerbose("bounding_box/debug", + // false); this->computeDebugOrientedBoundingBox = + // this->getParamVerbose("oriented_bounding_box/debug", false); + // this->computeDebugLocalBoundingBox = + // this->getParamVerbose("local_bounding_box/debug", false); + // this->publishBoundingSphereMarker = + // this->getParamVerbose("bounding_sphere/marker", false); + // this->publishBoundingBoxMarker = + // this->getParamVerbose("bounding_box/marker", false); + // this->publishOrientedBoundingBoxMarker = + // this->getParamVerbose("oriented_bounding_box/marker", false); + // this->publishLocalBoundingBoxMarker = + // this->getParamVerbose("local_bounding_box/marker", false); + // this->localBoundingBoxFrame = + // this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); + // this->publishDebugPclInside = this->getParamVerbose("debug/pcl/inside", + // false); this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", + // false); this->publishDebugPclShadow = + // this->getParamVerbose("debug/pcl/shadow", false); + // this->publishDebugContainsMarker = + // this->getParamVerbose("debug/marker/contains", false); + // this->publishDebugShadowMarker = + // this->getParamVerbose("debug/marker/shadow", false); + // this->publishDebugBsphereMarker = + // this->getParamVerbose("debug/marker/bounding_sphere", false); + // this->publishDebugBboxMarker = + // this->getParamVerbose("debug/marker/bounding_box", false); + + // const auto inflationPadding = + // this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); const auto + // inflationScale = this->getParamVerbose("body_model/inflation/scale", 1.0); + // this->defaultContainsInflation.padding = + // this->getParamVerbose("body_model/inflation/contains_test/padding", + // inflationPadding, "m"); this->defaultContainsInflation.scale = + // this->getParamVerbose("body_model/inflation/contains_test/scale", + // inflationScale); this->defaultShadowInflation.padding = + // this->getParamVerbose("body_model/inflation/shadow_test/padding", + // inflationPadding, "m"); this->defaultShadowInflation.scale = + // this->getParamVerbose("body_model/inflation/shadow_test/scale", + // inflationScale); this->defaultBsphereInflation.padding = + // this->getParamVerbose("body_model/inflation/bounding_sphere/padding", + // inflationPadding, "m"); this->defaultBsphereInflation.scale = + // this->getParamVerbose("body_model/inflation/bounding_sphere/scale", + // inflationScale); this->defaultBboxInflation.padding = + // this->getParamVerbose("body_model/inflation/bounding_box/padding", + // inflationPadding, "m"); this->defaultBboxInflation.scale = + // this->getParamVerbose("body_model/inflation/bounding_box/scale", + // inflationScale); // read per-link padding - // const auto perLinkInflationPadding = this->getParamVerboseMap("body_model/inflation/per_link/padding", std::map(), "m"); - // for (const auto& inflationPair : perLinkInflationPadding) + // const auto perLinkInflationPadding = + // this->getParamVerboseMap("body_model/inflation/per_link/padding", + // std::map(), "m"); for (const auto& inflationPair : + // perLinkInflationPadding) // { // bool containsOnly; // bool shadowOnly; @@ -125,21 +170,27 @@ bool RobotBodyFilter::configure() { // if (!shadowOnly && !bsphereOnly && !bboxOnly) // this->perLinkContainsInflation[linkName] = - // ScaleAndPadding(this->defaultContainsInflation.scale, inflationPair.second); + // ScaleAndPadding(this->defaultContainsInflation.scale, + // inflationPair.second); // if (!containsOnly && !bsphereOnly && !bboxOnly) // this->perLinkShadowInflation[linkName] = - // ScaleAndPadding(this->defaultShadowInflation.scale, inflationPair.second); + // ScaleAndPadding(this->defaultShadowInflation.scale, + // inflationPair.second); // if (!containsOnly && !shadowOnly && !bboxOnly) // this->perLinkBsphereInflation[linkName] = - // ScaleAndPadding(this->defaultBsphereInflation.scale, inflationPair.second); + // ScaleAndPadding(this->defaultBsphereInflation.scale, + // inflationPair.second); // if (!containsOnly && !shadowOnly && !bsphereOnly) // this->perLinkBboxInflation[linkName] = - // ScaleAndPadding(this->defaultBboxInflation.scale, inflationPair.second); + // ScaleAndPadding(this->defaultBboxInflation.scale, + // inflationPair.second); // } // read per-link scale - // const auto perLinkInflationScale = this->getParamVerboseMap("body_model/inflation/per_link/scale", std::map()); - // for (const auto& inflationPair : perLinkInflationScale) + // const auto perLinkInflationScale = + // this->getParamVerboseMap("body_model/inflation/per_link/scale", + // std::map()); for (const auto& inflationPair : + // perLinkInflationScale) // { // bool containsOnly; // bool shadowOnly; @@ -154,167 +205,234 @@ bool RobotBodyFilter::configure() { // if (!shadowOnly && !bsphereOnly && !bboxOnly) // { - // if (this->perLinkContainsInflation.find(linkName) == this->perLinkContainsInflation.end()) - // this->perLinkContainsInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultContainsInflation.padding); + // if (this->perLinkContainsInflation.find(linkName) == + // this->perLinkContainsInflation.end()) + // this->perLinkContainsInflation[linkName] = + // ScaleAndPadding(inflationPair.second, + // this->defaultContainsInflation.padding); // else - // this->perLinkContainsInflation[linkName].scale = inflationPair.second; + // this->perLinkContainsInflation[linkName].scale = + // inflationPair.second; // } // if (!containsOnly && !bsphereOnly && !bboxOnly) // { - // if (this->perLinkShadowInflation.find(linkName) == this->perLinkShadowInflation.end()) - // this->perLinkShadowInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultShadowInflation.padding); + // if (this->perLinkShadowInflation.find(linkName) == + // this->perLinkShadowInflation.end()) + // this->perLinkShadowInflation[linkName] = + // ScaleAndPadding(inflationPair.second, + // this->defaultShadowInflation.padding); // else // this->perLinkShadowInflation[linkName].scale = inflationPair.second; // } // if (!containsOnly && !shadowOnly && !bboxOnly) // { - // if (this->perLinkBsphereInflation.find(linkName) == this->perLinkBsphereInflation.end()) - // this->perLinkBsphereInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBsphereInflation.padding); + // if (this->perLinkBsphereInflation.find(linkName) == + // this->perLinkBsphereInflation.end()) + // this->perLinkBsphereInflation[linkName] = + // ScaleAndPadding(inflationPair.second, + // this->defaultBsphereInflation.padding); // else // this->perLinkBsphereInflation[linkName].scale = inflationPair.second; // } // if (!containsOnly && !shadowOnly && !bsphereOnly) // { - // if (this->perLinkBboxInflation.find(linkName) == this->perLinkBboxInflation.end()) - // this->perLinkBboxInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBboxInflation.padding); + // if (this->perLinkBboxInflation.find(linkName) == + // this->perLinkBboxInflation.end()) + // this->perLinkBboxInflation[linkName] = + // ScaleAndPadding(inflationPair.second, + // this->defaultBboxInflation.padding); // else // this->perLinkBboxInflation[linkName].scale = inflationPair.second; // } // } - // can contain either whole link names, or scoped names of their collisions (i.e. "link::collision_1" or "link::my_collision") - // this->linksIgnoredInBoundingSphere = this->template getParamVerboseSet("ignored_links/bounding_sphere"); - // this->linksIgnoredInBoundingBox = this->template getParamVerboseSet("ignored_links/bounding_box"); - // this->linksIgnoredInContainsTest = this->template getParamVerboseSet("ignored_links/contains_test"); - // this->linksIgnoredInShadowTest = this->template getParamVerboseSet("ignored_links/shadow_test", { "laser" }); - // this->linksIgnoredEverywhere = this->template getParamVerboseSet("ignored_links/everywhere"); - // this->onlyLinks = this->template getParamVerboseSet("only_links"); - - // this->robotDescriptionUpdatesFieldName = this->getParamVerbose("body_model/dynamic_robot_description/field_name", "robot_model"); + // can contain either whole link names, or scoped names of their collisions + // (i.e. "link::collision_1" or "link::my_collision") + // this->linksIgnoredInBoundingSphere = this->template + // getParamVerboseSet("ignored_links/bounding_sphere"); + // this->linksIgnoredInBoundingBox = this->template + // getParamVerboseSet("ignored_links/bounding_box"); + // this->linksIgnoredInContainsTest = this->template + // getParamVerboseSet("ignored_links/contains_test"); + // this->linksIgnoredInShadowTest = this->template + // getParamVerboseSet("ignored_links/shadow_test", { "laser" }); + // this->linksIgnoredEverywhere = this->template + // getParamVerboseSet("ignored_links/everywhere"); this->onlyLinks = + // this->template getParamVerboseSet("only_links"); + + // this->robotDescriptionUpdatesFieldName = + // this->getParamVerbose("body_model/dynamic_robot_description/field_name", + // "robot_model"); // // subscribe for robot_description param changes // this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( - // "dynamic_robot_model_server/parameter_updates", 10, &RobotBodyFilter::robotDescriptionUpdated, this); + // "dynamic_robot_model_server/parameter_updates", 10, + // &RobotBodyFilter::robotDescriptionUpdated, this); - // this->reloadRobotModelServiceServer = this->privateNodeHandle.advertiseService( + // this->reloadRobotModelServiceServer = + // this->privateNodeHandle.advertiseService( // "reload_model", &RobotBodyFilter::triggerModelReload, this); // if (this->computeBoundingSphere) { - // this->boundingSpherePublisher = this->nodeHandle.template advertise("robot_bounding_sphere", 100); + // this->boundingSpherePublisher = + // this->nodeHandle.template advertise( + // "robot_bounding_sphere", 100); // } // if (this->computeBoundingBox) { - // this->boundingBoxPublisher = this->nodeHandle.template advertise("robot_bounding_box", 100); + // this->boundingBoxPublisher = this->nodeHandle.template + // advertise("robot_bounding_box", 100); // } // if (this->computeOrientedBoundingBox) { - // this->orientedBoundingBoxPublisher = this->nodeHandle.template advertise("robot_oriented_bounding_box", 100); + // this->orientedBoundingBoxPublisher = this->nodeHandle.template + // advertise("robot_oriented_bounding_box", + // 100); // } // if (this->computeLocalBoundingBox) { - // this->localBoundingBoxPublisher = this->nodeHandle.template advertise("robot_local_bounding_box", 100); + // this->localBoundingBoxPublisher = this->nodeHandle.template + // advertise("robot_local_bounding_box", + // 100); // } // if (this->publishBoundingSphereMarker && this->computeBoundingSphere) { - // this->boundingSphereMarkerPublisher = this->nodeHandle.template advertise("robot_bounding_sphere_marker", 100); + // this->boundingSphereMarkerPublisher = this->nodeHandle.template + // advertise("robot_bounding_sphere_marker", + // 100); // } // if (this->publishBoundingBoxMarker && this->computeBoundingBox) { - // this->boundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_bounding_box_marker", 100); + // this->boundingBoxMarkerPublisher = this->nodeHandle.template + // advertise("robot_bounding_box_marker", + // 100); // } - // if (this->publishOrientedBoundingBoxMarker && this->computeOrientedBoundingBox) { - // this->orientedBoundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_oriented_bounding_box_marker", 100); + // if (this->publishOrientedBoundingBoxMarker && + // this->computeOrientedBoundingBox) { + // this->orientedBoundingBoxMarkerPublisher = this->nodeHandle.template + // advertise("robot_oriented_bounding_box_marker", + // 100); // } // if (this->publishLocalBoundingBoxMarker && this->computeLocalBoundingBox) { - // this->localBoundingBoxMarkerPublisher = this->nodeHandle.template advertise("robot_local_bounding_box_marker", 100); + // this->localBoundingBoxMarkerPublisher = this->nodeHandle.template + // advertise("robot_local_bounding_box_marker", + // 100); // } // if (this->publishNoBoundingBoxPointcloud) // { - // this->scanPointCloudNoBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_bbox", 100); + // this->scanPointCloudNoBoundingBoxPublisher = this->nodeHandle.template + // advertise("scan_point_cloud_no_bbox", + // 100); // } // if (this->publishNoOrientedBoundingBoxPointcloud) // { - // this->scanPointCloudNoOrientedBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_oriented_bbox", 100); + // this->scanPointCloudNoOrientedBoundingBoxPublisher = + // this->nodeHandle.template + // advertise("scan_point_cloud_no_oriented_bbox", + // 100); // } // if (this->publishNoLocalBoundingBoxPointcloud) // { - // this->scanPointCloudNoLocalBoundingBoxPublisher = this->nodeHandle.template advertise("scan_point_cloud_no_local_bbox", 100); + // this->scanPointCloudNoLocalBoundingBoxPublisher = + // this->nodeHandle.template + // advertise("scan_point_cloud_no_local_bbox", + // 100); // } // if (this->publishNoBoundingSpherePointcloud) // { - // this->scanPointCloudNoBoundingSpherePublisher = this->nodeHandle.template advertise("scan_point_cloud_no_bsphere", 100); + // this->scanPointCloudNoBoundingSpherePublisher = this->nodeHandle.template + // advertise("scan_point_cloud_no_bsphere", + // 100); // } // if (this->publishDebugPclInside) // { - // this->debugPointCloudInsidePublisher = this->nodeHandle.template advertise("scan_point_cloud_inside", 100); + // this->debugPointCloudInsidePublisher = this->nodeHandle.template + // advertise("scan_point_cloud_inside", 100); // } // if (this->publishDebugPclClip) // { - // this->debugPointCloudClipPublisher = this->nodeHandle.template advertise("scan_point_cloud_clip", 100); + // this->debugPointCloudClipPublisher = this->nodeHandle.template + // advertise("scan_point_cloud_clip", 100); // } // if (this->publishDebugPclShadow) // { - // this->debugPointCloudShadowPublisher = this->nodeHandle.template advertise("scan_point_cloud_shadow", 100); + // this->debugPointCloudShadowPublisher = this->nodeHandle.template + // advertise("scan_point_cloud_shadow", 100); // } // if (this->publishDebugContainsMarker) // { - // this->debugContainsMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_contains_test", 100); + // this->debugContainsMarkerPublisher = this->nodeHandle.template + // advertise("robot_model_for_contains_test", + // 100); // } // if (this->publishDebugShadowMarker) // { - // this->debugShadowMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_shadow_test", 100); + // this->debugShadowMarkerPublisher = this->nodeHandle.template + // advertise("robot_model_for_shadow_test", + // 100); // } // if (this->publishDebugBsphereMarker) // { - // this->debugBsphereMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_bounding_sphere", 100); + // this->debugBsphereMarkerPublisher = this->nodeHandle.template + // advertise("robot_model_for_bounding_sphere", + // 100); // } // if (this->publishDebugBboxMarker) // { - // this->debugBboxMarkerPublisher = this->nodeHandle.template advertise("robot_model_for_bounding_box", 100); + // this->debugBboxMarkerPublisher = this->nodeHandle.template + // advertise("robot_model_for_bounding_box", + // 100); // } // if (this->computeDebugBoundingBox) { - // this->boundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( + // this->boundingBoxDebugMarkerPublisher = this->nodeHandle.template + // advertise( // "robot_bounding_box_debug", 100); // } // if (this->computeDebugOrientedBoundingBox) { - // this->orientedBoundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( + // this->orientedBoundingBoxDebugMarkerPublisher = this->nodeHandle.template + // advertise( // "robot_oriented_bounding_box_debug", 100); // } // if (this->computeDebugLocalBoundingBox) { - // this->localBoundingBoxDebugMarkerPublisher = this->nodeHandle.template advertise( + // this->localBoundingBoxDebugMarkerPublisher = this->nodeHandle.template + // advertise( // "robot_local_bounding_box_debug", 100); // } // if (this->computeDebugBoundingSphere) { - // this->boundingSphereDebugMarkerPublisher = this->nodeHandle.template advertise( + // this->boundingSphereDebugMarkerPublisher = this->nodeHandle.template + // advertise( // "robot_bounding_sphere_debug", 100); // } // initialize the 3D body masking tool - // auto getShapeTransformCallback = std::bind(&RobotBodyFilter::getShapeTransform, this, std::placeholders::_1, std::placeholders::_2); - // shapeMask = std::make_unique(getShapeTransformCallback, + // auto getShapeTransformCallback = + // std::bind(&RobotBodyFilter::getShapeTransform, this, std::placeholders::_1, + // std::placeholders::_2); shapeMask = + // std::make_unique(getShapeTransformCallback, // this->minDistance, this->maxDistance, // doClipping, doContainsTest, doShadowTest, maxShadowDistance); - // // the other case happens when configure() is called again from update() (e.g. when a new bag file + // // the other case happens when configure() is called again from update() + // (e.g. when a new bag file // // started playing) // if (this->tfFramesWatchdog == nullptr) { // std::set initialMonitoredFrames; @@ -323,81 +441,103 @@ bool RobotBodyFilter::configure() { // initialMonitoredFrames.insert(this->sensorFrame); // } - // this->tfFramesWatchdog = std::make_shared(this->filteringFrame, + // this->tfFramesWatchdog = + // std::make_shared(this->filteringFrame, // initialMonitoredFrames, this->tfBuffer, - // this->unreachableTransformTimeout, ros::Rate(ros::Duration(1.0))); + // this->unreachableTransformTimeout, + // rclcpp::Rate(rclcpp::Duration(1.0))); // this->tfFramesWatchdog->start(); // } // { // initialize the robot body to be masked out // string robotUrdf; - // while (!this->nodeHandle.getParam(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { + // while (!this->nodeHandle.getParam(this->robotDescriptionParam, robotUrdf) + // || robotUrdf.length() == 0) { // if (this->failWithoutRobotDescription) // { // throw std::runtime_error( - // "RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); + // "RobotBodyFilter: " + this->robotDescriptionParam + " is empty or + // not set."); // } - // if (!ros::ok()) + // if (!rclcpp::ok()) // return false; - // ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide the robot model. Waiting 1s.", + // ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide the + // robot model. Waiting 1s.", // robotDescriptionParam.c_str()); - // ros::Duration(1.0).sleep(); + // rclcpp::Duration(1.0).sleep(); // } - // // happens when configure() is called again from update() (e.g. when a new bag file started + // // happens when configure() is called again from update() (e.g. when a + // new bag file started // // playing) // if (!this->shapesToLinks.empty()) // this->clearRobotMask(); // this->addRobotMaskFromUrdf(robotUrdf); // } - // ROS_INFO("RobotBodyFilter: Successfully configured."); - // ROS_INFO("Filtering data in frame %s", this->filteringFrame.c_str()); - // ROS_INFO("RobotBodyFilter: Filtering into the following categories:"); - // ROS_INFO("RobotBodyFilter: \tOUTSIDE"); - // if (doClipping) ROS_INFO("RobotBodyFilter: \tCLIP"); - // if (doContainsTest) ROS_INFO("RobotBodyFilter: \tINSIDE"); - // if (doShadowTest) ROS_INFO("RobotBodyFilter: \tSHADOW"); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Successfully + // configured."); RCLCPP_INFO(nodeHandle.get_logger(),"Filtering data in frame + // %s", this->filteringFrame.c_str()); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering into the + // following categories:"); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: \tOUTSIDE"); + // if (doClipping) RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: + // \tCLIP"); if (doContainsTest) + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: \tINSIDE"); if + // (doShadowTest) RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: + // \tSHADOW"); // if (this->onlyLinks.empty()) { // if (this->linksIgnoredEverywhere.empty()) { - // ROS_INFO("RobotBodyFilter: Filtering applied to all links."); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied + // to all links."); // } else { - // ROS_INFO("RobotBodyFilter: Filtering applied to all links except %s.", to_string(this->linksIgnoredEverywhere).c_str()); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied + // to all links except %s.", + // to_string(this->linksIgnoredEverywhere).c_str()); // } // } else { // if (this->linksIgnoredEverywhere.empty()) { - // ROS_INFO("RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied + // to links %s.", to_string(this->onlyLinks).c_str()); // } else { - // ROS_INFO("RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", to_string(this->onlyLinks).c_str(), to_string(this->linksIgnoredEverywhere).c_str()); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied + // to links %s with these links excluded: %s.", + // to_string(this->onlyLinks).c_str(), + // to_string(this->linksIgnoredEverywhere).c_str()); // } // } - // this->timeConfigured = ros::Time::now(); + // this->timeConfigured = rclcpp::Time::now(); return true; } bool RobotBodyFilterLaserScan::configure() { - // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", true); + // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", + // true); // bool success = RobotBodyFilter::configure(); return false; } bool RobotBodyFilterPointCloud2::configure() { - // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", false); + // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", + // false); // bool success = RobotBodyFilter::configure(); // if (!success) // return false; - // this->outputFrame = this->getParamVerbose("frames/output", this->filteringFrame); + // this->outputFrame = this->getParamVerbose("frames/output", + // this->filteringFrame); - // const auto pointChannels = this->getParamVerbose("cloud/point_channels", std::vector{"vp_"}); - // const auto directionChannels = this->getParamVerbose("cloud/direction_channels", std::vector{"normal_"}); + // const auto pointChannels = this->getParamVerbose("cloud/point_channels", + // std::vector{"vp_"}); const auto directionChannels = + // this->getParamVerbose("cloud/direction_channels", + // std::vector{"normal_"}); // for (const auto& channel : pointChannels) // this->channelsToTransform[channel] = CloudChannelType::POINT; @@ -418,22 +558,24 @@ bool RobotBodyFilter::computeMask( // this->modelMutex has to be already locked! const clock_t stopwatchOverall = clock(); - const auto& scanTime = projectedPointCloud.header.stamp; + const auto &scanTime = projectedPointCloud.header.stamp; // compute a mask of point indices for points from projectedPointCloud // that tells if they are inside or outside robot, or shadow points - if (!this->pointByPointScan) - { + if (!this->pointByPointScan) { Eigen::Vector3d sensorPosition; try { const auto sensorTf = this->tfBuffer->lookupTransform( this->filteringFrame, sensorFrame, scanTime, remainingTime(scanTime, this->reachableTransformTimeout)); tf2::fromMsg(sensorTf.transform.translation, sensorPosition); - } catch (tf2::TransformException& e) { - RCLCPP_ERROR(nodeHandle.get_logger(), "RobotBodyFilter: Could not compute filtering mask due to this " - "TF exception: %s", e.what()); + } catch (tf2::TransformException &e) { + RCLCPP_ERROR( + nodeHandle.get_logger(), + "RobotBodyFilter: Could not compute filtering mask due to this " + "TF exception: %s", + e.what()); return false; } @@ -442,7 +584,8 @@ bool RobotBodyFilter::computeMask( // updates shapes according to tf cache (by calling getShapeTransform // for each shape) and masks contained points - this->shapeMask->maskContainmentAndShadows(projectedPointCloud, pointMask, sensorPosition); + this->shapeMask->maskContainmentAndShadows(projectedPointCloud, pointMask, + sensorPosition); } else { CloudConstIter x_it(projectedPointCloud, "x"); CloudConstIter y_it(projectedPointCloud, "y"); @@ -455,18 +598,23 @@ bool RobotBodyFilter::computeMask( pointMask.resize(num_points(projectedPointCloud)); double scanDuration = 0.0; - for (CloudConstIter stamps_end_it(projectedPointCloud, "stamps"); stamps_end_it != stamps_end_it.end(); ++stamps_end_it) - { + for (CloudConstIter stamps_end_it(projectedPointCloud, "stamps"); + stamps_end_it != stamps_end_it.end(); ++stamps_end_it) { if ((*stamps_end_it) > static_cast(scanDuration)) scanDuration = static_cast(*stamps_end_it); } - const rclcpp::Time afterScanTime(scanTime + rclcpp::Duration::from_seconds(scanDuration)); + + const rclcpp::Time afterScanTime( + rclcpp::Time(scanTime) + rclcpp::Duration::from_seconds(scanDuration)); size_t updateBodyPosesEvery; - if (this->modelPoseUpdateInterval.sec == 0 && this->modelPoseUpdateInterval.nsec == 0) { + if (this->modelPoseUpdateInterval.sec == 0 && + this->modelPoseUpdateInterval.nsec == 0) { updateBodyPosesEvery = 1; } else { - updateBodyPosesEvery = static_cast(ceil(this->modelPoseUpdateInterval.toSec() / scanDuration * num_points(projectedPointCloud))); + updateBodyPosesEvery = static_cast( + ceil(this->modelPoseUpdateInterval.toSec() / scanDuration * + num_points(projectedPointCloud))); // prevent division by zero if (updateBodyPosesEvery == 0) updateBodyPosesEvery = 1; @@ -476,9 +624,11 @@ bool RobotBodyFilter::computeMask( // isn't really taken point by point with different timestamps if (scanDuration == 0.0) { updateBodyPosesEvery = num_points(projectedPointCloud) + 1; - RCLCPP_WARN_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: sensor/point_by_point is set to true but " - "all points in the cloud have the same timestamp. You should" - " change the parameter to false to gain performance."); + RCLCPP_WARN_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: sensor/point_by_point is set to true but " + "all points in the cloud have the same timestamp. You should" + " change the parameter to false to gain performance."); } // update transforms cache, which is then used in body masking @@ -489,8 +639,8 @@ bool RobotBodyFilter::computeMask( RayCastingShapeMask::MaskValue mask; this->cacheLookupBetweenScansRatio = 0.0; - for (size_t i = 0; i < num_points(projectedPointCloud); ++i, ++x_it, ++y_it, ++z_it, ++vp_x_it, ++vp_y_it, ++vp_z_it, ++stamps_it) - { + for (size_t i = 0; i < num_points(projectedPointCloud); ++i, ++x_it, ++y_it, + ++z_it, ++vp_x_it, ++vp_y_it, ++vp_z_it, ++stamps_it) { point.x() = *x_it; point.y() = *y_it; point.z() = *z_it; @@ -503,16 +653,20 @@ bool RobotBodyFilter::computeMask( const auto updateBodyPoses = i % updateBodyPosesEvery == 0; if (updateBodyPoses && scanDuration > 0.0) - this->cacheLookupBetweenScansRatio = static_cast(*stamps_it) / scanDuration; + this->cacheLookupBetweenScansRatio = + static_cast(*stamps_it) / scanDuration; // updates shapes according to tf cache (by calling getShapeTransform // for each shape) and masks contained points - this->shapeMask->maskContainmentAndShadows(point, mask, viewPoint, updateBodyPoses); + this->shapeMask->maskContainmentAndShadows(point, mask, viewPoint, + updateBodyPoses); pointMask[i] = mask; } } - RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Mask computed in %.5f secs.", + double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); this->publishDebugPointClouds(projectedPointCloud, pointMask); this->publishDebugMarkers(scanTime); @@ -521,29 +675,45 @@ bool RobotBodyFilter::computeMask( this->computeAndPublishOrientedBoundingBox(projectedPointCloud); this->computeAndPublishLocalBoundingBox(projectedPointCloud); - RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Filtering run time is %.5f secs.", + double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); return true; } -bool RobotBodyFilterLaserScan::update(const LaserScan &inputScan, LaserScan &filteredScan) { - const auto& scanTime = inputScan.header.stamp; +bool RobotBodyFilterLaserScan::update( + const sensor_msgs::msg::LaserScan &inputScan, + sensor_msgs::msg::LaserScan &filteredScan) { + const auto &headerScanTime = inputScan.header.stamp; + const auto &scanTime = rclcpp::Time(inputScan.header.stamp); if (!this->configured_) { - ROS_DEBUG("RobotBodyFilter: Ignore scan from time %u.%u - filter not yet initialized.", - scanTime.sec, scanTime.nsec); + RCLCPP_DEBUG( + nodeHandle.get_logger(), + "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) >= timeConfigured)) { - ROS_DEBUG("RobotBodyFilter: Ignore scan from time %u.%u - filter not yet initialized.", - scanTime.sec, scanTime.nsec); + if ((scanTime < timeConfigured) && + ((scanTime + tfBufferLength) >= timeConfigured)) { + RCLCPP_DEBUG( + nodeHandle.get_logger(), + "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) < timeConfigured)) { - ROS_WARN("RobotBodyFilter: Old TF data received. Clearing TF buffer and reconfiguring laser" - "filter. If you're replaying a bag file, make sure rosparam /use_sim_time is set to " - "true"); + if ((scanTime < timeConfigured) && + ((scanTime + tfBufferLength) < timeConfigured)) { + RCLCPP_WARN(nodeHandle.get_logger(), + "RobotBodyFilter: Old TF data received. Clearing TF buffer and " + "reconfiguring laser" + "filter. If you're replaying a bag file, make sure rosparam " + "/use_sim_time is set to " + "true"); this->configure(); return false; } @@ -551,27 +721,34 @@ bool RobotBodyFilterLaserScan::update(const LaserScan &inputScan, LaserScan &fil // tf2 doesn't like frames starting with slash const auto scanFrame = stripLeadingSlash(inputScan.header.frame_id, true); - // Passing a sensorFrame does not make sense. Scan messages can't be transformed to other frames. + // Passing a sensorFrame does not make sense. Scan messages can't be + // transformed to other frames. if (!this->sensorFrame.empty() && this->sensorFrame != scanFrame) { - ROS_WARN_ONCE("RobotBodyFilter: frames/sensor is set to frame_id '%s' different than " - "the frame_id of the incoming message '%s'. This is an invalid configuration: " - "the frames/sensor parameter will be neglected.", - this->sensorFrame.c_str(), scanFrame.c_str()); + RCLCPP_WARN_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: frames/sensor is set to frame_id '%s' different than " + "the frame_id of the incoming message '%s'. This is an invalid " + "configuration: " + "the frames/sensor parameter will be neglected.", + this->sensorFrame.c_str(), scanFrame.c_str()); } - if (!this->tfFramesWatchdog->isReachable(scanFrame)) - { - ROS_DEBUG("RobotBodyFilter: Throwing away scan since sensor frame is unreachable."); - // if this->sensorFrame is empty, it can happen that we're not actually monitoring the sensor - // frame, so start monitoring it + if (!this->tfFramesWatchdog->isReachable(scanFrame)) { + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Throwing away scan since sensor frame is " + "unreachable."); + // if this->sensorFrame is empty, it can happen that we're not actually + // monitoring the sensor frame, so start monitoring it if (!this->tfFramesWatchdog->isMonitored(scanFrame)) this->tfFramesWatchdog->addMonitoredFrame(scanFrame); return false; } - if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable()) - { - ROS_DEBUG("RobotBodyFilter: Throwing away scan since not all frames are reachable."); + if (this->requireAllFramesReachable && + !this->tfFramesWatchdog->areAllFramesReachable()) { + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Throwing away scan since not all frames are " + "reachable."); return false; } @@ -580,91 +757,118 @@ bool RobotBodyFilterLaserScan::update(const LaserScan &inputScan, LaserScan &fil // create the output copy of the input scan filteredScan = inputScan; filteredScan.header.frame_id = scanFrame; - filteredScan.range_min = max(inputScan.range_min, (float) this->minDistance); + filteredScan.range_min = fmax(inputScan.range_min, (float)this->minDistance); if (this->maxDistance > 0.0) - filteredScan.range_max = min(inputScan.range_max, (float) this->maxDistance); + filteredScan.range_max = + fmin(inputScan.range_max, (float)this->maxDistance); { // acquire the lock here, because we work with the tfBuffer all the time std::lock_guard guard(*this->modelMutex); - if (this->pointByPointScan) - { // make sure we have all the tfs between sensor frame and fixedFrame during the time of scan acquisition - const auto scanDuration = inputScan.ranges.size() * inputScan.time_increment; - const auto afterScanTime = scanTime + ros::Duration().fromSec(scanDuration); - - string err; - if (!this->tfBuffer->canTransform(this->fixedFrame, scanFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err) || - !this->tfBuffer->canTransform(this->fixedFrame, scanFrame, afterScanTime, - remainingTime(afterScanTime, this->reachableTransformTimeout), &err)) { - if (err.find("future") != string::npos) { - const auto delay = ros::Time::now() - scanTime; - ROS_ERROR_THROTTLE(3, "RobotBodyFilter: Cannot transform laser scan to " - "fixed frame. The scan is too much delayed (%s s). TF error: %s", - to_string(delay).c_str(), err.c_str()); + if (this->pointByPointScan) { // make sure we have all the tfs between + // sensor frame and fixedFrame during the time + // of scan acquisition + const auto scanDuration = + inputScan.ranges.size() * inputScan.time_increment; + const auto afterScanTime = + scanTime + rclcpp::Duration::from_seconds(scanDuration); + + std::string err; + if (!this->tfBuffer->canTransform( + this->fixedFrame, scanFrame, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err) || + !this->tfBuffer->canTransform( + this->fixedFrame, scanFrame, afterScanTime, + remainingTime(afterScanTime, this->reachableTransformTimeout), + &err)) { + if (err.find("future") != std::string::npos) { + const auto delay = nodeHandle.now() - scanTime; + auto &clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE( + nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform laser scan to " + "fixed frame. The scan is too much delayed (%s s). TF error: %s", + to_string(delay).c_str(), err.c_str()); } else { - ROS_ERROR_DELAYED_THROTTLE(3, "RobotBodyFilter: Cannot transform laser scan to " - "fixed frame. Something's wrong with TFs: %s", err.c_str()); + auto &clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE( + nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform laser scan to " + "fixed frame. Something's wrong with TFs: %s", + err.c_str()); } return false; } } - // The point cloud will have fields x, y, z, intensity (float32) and index (int32) - // and for point-by-point scans also timestamp and viewpoint - sensor_msgs::PointCloud2 projectedPointCloud; + // The point cloud will have fields x, y, z, intensity (float32) and index + // (int32) and for point-by-point scans also timestamp and viewpoint + sensor_msgs::msg::PointCloud2 projectedPointCloud; { // project the scan measurements to a point cloud in the filteringFrame - sensor_msgs::PointCloud2 tmpPointCloud; + sensor_msgs::msg::PointCloud2 tmpPointCloud; - // the projected point cloud can omit some measurements if they are out of the defined scan's range; - // for this case, the second channel ("index") contains indices of the point cloud's points into the scan - auto channelOptions = - laser_geometry::channel_option::Intensity | - laser_geometry::channel_option::Index; + // the projected point cloud can omit some measurements if they are out of + // the defined scan's range; for this case, the second channel ("index") + // contains indices of the point cloud's points into the scan + auto channelOptions = laser_geometry::channel_option::Intensity | + laser_geometry::channel_option::Index; if (this->pointByPointScan) { - ROS_INFO_ONCE("RobotBodyFilter: Applying complex laser scan projection."); + RCLCPP_INFO_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: Applying complex laser scan projection."); // perform the complex laser scan projection - channelOptions |= - laser_geometry::channel_option::Timestamp | - laser_geometry::channel_option::Viewpoint; + channelOptions |= laser_geometry::channel_option::Timestamp | + laser_geometry::channel_option::Viewpoint; laserProjector.transformLaserScanToPointCloud( - this->fixedFrame, inputScan, tmpPointCloud, *this->tfBuffer, - -1, channelOptions); + this->fixedFrame, inputScan, tmpPointCloud, *this->tfBuffer, -1, + channelOptions); } else { - ROS_INFO_ONCE("RobotBodyFilter: Applying simple laser scan projection."); + RCLCPP_INFO_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: Applying simple laser scan projection."); // perform simple laser scan projection laserProjector.projectLaser(inputScan, tmpPointCloud, -1.0, - channelOptions); + channelOptions); } // convert to filtering frame if (tmpPointCloud.header.frame_id == this->filteringFrame) { projectedPointCloud = std::move(tmpPointCloud); } else { - ROS_INFO_ONCE("RobotBodyFilter: Transforming scan from frame %s to %s", - tmpPointCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); + RCLCPP_INFO_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: Transforming scan from frame %s to %s", + tmpPointCloud.header.frame_id.c_str(), + this->filteringFrame.c_str()); std::string err; - if (!this->tfBuffer->canTransform(this->filteringFrame, - tmpPointCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - ROS_ERROR_DELAYED_THROTTLE(3, "RobotBodyFilter: Cannot transform " - "laser scan to filtering frame. Something's wrong with TFs: %s", - err.c_str()); + if (!this->tfBuffer->canTransform( + this->filteringFrame, tmpPointCloud.header.frame_id, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), + &err)) { + // RCLCPP_ERROR_DELAYED_THROTTLE( + // nodeHandle.get_logger(), 3, + // "RobotBodyFilter: Cannot transform " + // "laser scan to filtering frame. Something's wrong with TFs: + // %s", err.c_str()); return false; } transformWithChannels(tmpPointCloud, projectedPointCloud, - *this->tfBuffer, this->filteringFrame, this->channelsToTransform); + *this->tfBuffer, this->filteringFrame, + this->channelsToTransform); } } - ROS_DEBUG("RobotBodyFilter: Scan transformation run time is %.5f secs.", double(clock()-stopwatchOverall) / CLOCKS_PER_SEC); + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Scan transformation run time is %.5f secs.", + double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); - vector pointMask; - const auto success = this->computeMask(projectedPointCloud, pointMask, scanFrame); + std::vector pointMask; + const auto success = + this->computeMask(projectedPointCloud, pointMask, scanFrame); if (!success) return false; @@ -672,26 +876,28 @@ bool RobotBodyFilterLaserScan::update(const LaserScan &inputScan, LaserScan &fil { // remove invalid points const float INVALID_POINT_VALUE = std::numeric_limits::quiet_NaN(); try { - sensor_msgs::PointCloud2Iterator indexIt(projectedPointCloud, "index"); + sensor_msgs::msg::PointCloud2Iterator indexIt(projectedPointCloud, + "index"); size_t indexInScan; for (const auto maskValue : pointMask) { switch (maskValue) { - case RayCastingShapeMask::MaskValue::INSIDE: - case RayCastingShapeMask::MaskValue::SHADOW: - case RayCastingShapeMask::MaskValue::CLIP: - indexInScan = static_cast(*indexIt); - filteredScan.ranges[indexInScan] = INVALID_POINT_VALUE; - break; - case RayCastingShapeMask::MaskValue::OUTSIDE: - break; + case RayCastingShapeMask::MaskValue::INSIDE: + case RayCastingShapeMask::MaskValue::SHADOW: + case RayCastingShapeMask::MaskValue::CLIP: + indexInScan = static_cast(*indexIt); + filteredScan.ranges[indexInScan] = INVALID_POINT_VALUE; + break; + case RayCastingShapeMask::MaskValue::OUTSIDE: + break; } ++indexIt; } - } - catch (std::runtime_error&) { - ROS_ERROR("RobotBodyFilter: projectedPointCloud doesn't have field called 'index'," - " but the algorithm relies on that."); + } catch (std::runtime_error &) { + RCLCPP_ERROR(this->nodeHandle.get_logger(), + "RobotBodyFilter: projectedPointCloud doesn't have field " + "called 'index'," + " but the algorithm relies on that."); return false; } } @@ -700,60 +906,80 @@ bool RobotBodyFilterLaserScan::update(const LaserScan &inputScan, LaserScan &fil return true; } -bool RobotBodyFilterPointCloud2::update(const sensor_msgs::PointCloud2 &inputCloud, - sensor_msgs::PointCloud2 &filteredCloud) -{ - const auto& scanTime = inputCloud.header.stamp; +bool RobotBodyFilterPointCloud2::update( + const sensor_msgs::msg::PointCloud2 &inputCloud, + sensor_msgs::msg::PointCloud2 &filteredCloud) { + const auto &headerScanTime = inputCloud.header.stamp; + const auto &scanTime = rclcpp::Time(headerScanTime); if (!this->configured_) { - ROS_DEBUG("RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet initialized.", - scanTime.sec, scanTime.nsec); + RCLCPP_DEBUG( + nodeHandle.get_logger(), + "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) >= this->timeConfigured)) { - ROS_DEBUG("RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet initialized.", - scanTime.sec, scanTime.nsec); + if ((scanTime < this->timeConfigured) && + ((scanTime + this->tfBufferLength) >= this->timeConfigured)) { + RCLCPP_DEBUG( + nodeHandle.get_logger(), + "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) < this->timeConfigured)) { - ROS_WARN("RobotBodyFilter: Old TF data received. Clearing TF buffer and " - "reconfiguring laser filter. If you're replaying a bag file, make " - "sure rosparam /use_sim_time is set to true"); + if ((scanTime < this->timeConfigured) && + ((scanTime + this->tfBufferLength) < this->timeConfigured)) { + RCLCPP_WARN( + nodeHandle.get_logger(), + "RobotBodyFilter: Old TF data received. Clearing TF buffer and " + "reconfiguring laser filter. If you're replaying a bag file, make " + "sure rosparam /use_sim_time is set to true"); this->configure(); return false; } - const auto inputCloudFrame = this->sensorFrame.empty() ? - stripLeadingSlash(inputCloud.header.frame_id, true) : this->sensorFrame; - - if (!this->tfFramesWatchdog->isReachable(inputCloudFrame)) - { - ROS_DEBUG("RobotBodyFilter: Throwing away scan since sensor frame is unreachable."); - // if this->sensorFrame is empty, it can happen that we're not actually monitoring the cloud - // frame, so start monitoring it + const auto inputCloudFrame = + this->sensorFrame.empty() + ? stripLeadingSlash(inputCloud.header.frame_id, true) + : this->sensorFrame; + + if (!this->tfFramesWatchdog->isReachable(inputCloudFrame)) { + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Throwing away scan since sensor frame is " + "unreachable."); + // if this->sensorFrame is empty, it can happen that we're not actually + // monitoring the cloud frame, so start monitoring it if (!this->tfFramesWatchdog->isMonitored(inputCloudFrame)) this->tfFramesWatchdog->addMonitoredFrame(inputCloudFrame); return false; } - if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable()) - { - ROS_DEBUG("RobotBodyFilter: Throwing away scan since not all frames are reachable."); + if (this->requireAllFramesReachable && + !this->tfFramesWatchdog->areAllFramesReachable()) { + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Throwing away scan since not all frames are " + "reachable."); return false; } bool hasStampsField = false; bool hasVpXField = false, hasVpYField = false, hasVpZField = false; - for (const auto& field : inputCloud.fields) { - if (field.name == "stamps" && field.datatype == sensor_msgs::PointField::FLOAT32) + for (const auto &field : inputCloud.fields) { + if (field.name == "stamps" && + field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasStampsField = true; - else if (field.name == "vp_x" && field.datatype == sensor_msgs::PointField::FLOAT32) + else if (field.name == "vp_x" && + field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasVpXField = true; - else if (field.name == "vp_y" && field.datatype == sensor_msgs::PointField::FLOAT32) + else if (field.name == "vp_y" && + field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasVpYField = true; - else if (field.name == "vp_z" && field.datatype == sensor_msgs::PointField::FLOAT32) + else if (field.name == "vp_z" && + field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasVpZField = true; } @@ -761,103 +987,128 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::PointCloud2 &inputClo if (this->pointByPointScan) { if (inputCloud.height != 1 && inputCloud.is_dense == 0) { - ROS_WARN_ONCE("RobotBodyFilter: The pointcloud seems to be an organized " - "pointcloud, which usually means it was captured all at once." - " Consider setting 'point_by_point_scan' to false to get a " - "more efficient computation."); + RCLCPP_WARN_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: The pointcloud seems to be an organized " + "pointcloud, which usually means it was captured all at once." + " Consider setting 'point_by_point_scan' to false to get a " + "more efficient computation."); } if (!hasStampsField || !hasVpXField || !hasVpYField || !hasVpZField) { throw std::runtime_error("A point-by-point scan has to contain float32" "fields 'stamps', 'vp_x', 'vp_y' and 'vp_z'."); } } else if (hasStampsField) { - ROS_WARN_ONCE("RobotBodyFilter: The pointcloud has a 'stamps' field, " - "which indicates each point was probably captured at a " - "different time instant. Consider setting parameter " - "'point_by_point_scan' to true to get correct results."); + RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: The pointcloud has a 'stamps' field, " + "which indicates each point was probably captured at a " + "different time instant. Consider setting parameter " + "'point_by_point_scan' to true to get correct results."); } else if (inputCloud.height == 1 && inputCloud.is_dense == 1) { - ROS_WARN_ONCE("RobotBodyFilter: The pointcloud is dense, which usually means" - " it was captured each point at a different time instant. " - "Consider setting 'point_by_point_scan' to true to get a more" - " accurate version."); + RCLCPP_WARN_ONCE( + nodeHandle.get_logger(), + "RobotBodyFilter: The pointcloud is dense, which usually means" + " it was captured each point at a different time instant. " + "Consider setting 'point_by_point_scan' to true to get a more" + " accurate version."); } // Transform to filtering frame - sensor_msgs::PointCloud2 transformedCloud; + sensor_msgs::msg::PointCloud2 transformedCloud; if (inputCloud.header.frame_id == this->filteringFrame) { transformedCloud = inputCloud; } else { - ROS_INFO_ONCE("RobotBodyFilter: Transforming cloud from frame %s to %s", - inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: Transforming cloud from frame %s to %s", + inputCloud.header.frame_id.c_str(), + this->filteringFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform(this->filteringFrame, - inputCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - ROS_ERROR_DELAYED_THROTTLE(3, "RobotBodyFilter: Cannot transform " + if (!this->tfBuffer->canTransform( + this->filteringFrame, inputCloud.header.frame_id, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + auto &clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE( + nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform " "point cloud to filtering frame. Something's wrong with TFs: %s", err.c_str()); return false; } - transformWithChannels(inputCloud, transformedCloud, *this->tfBuffer, this->filteringFrame, - this->channelsToTransform); + transformWithChannels(inputCloud, transformedCloud, *this->tfBuffer, + this->filteringFrame, this->channelsToTransform); } - // Compute the mask and use it (transform message only if sensorFrame is specified) - vector pointMask; + // Compute the mask and use it (transform message only if sensorFrame is + // specified) + std::vector pointMask; { std::lock_guard guard(*this->modelMutex); - const auto success = this->computeMask(transformedCloud, pointMask, inputCloudFrame); + const auto success = + this->computeMask(transformedCloud, pointMask, inputCloudFrame); if (!success) return false; } // Filter the cloud - sensor_msgs::PointCloud2 tmpCloud; - CREATE_FILTERED_CLOUD(transformedCloud, tmpCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::OUTSIDE)) + sensor_msgs::msg::PointCloud2 tmpCloud; + CREATE_FILTERED_CLOUD( + transformedCloud, tmpCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::OUTSIDE)) // Transform to output frame if (tmpCloud.header.frame_id == this->outputFrame) { filteredCloud = std::move(tmpCloud); } else { - ROS_INFO_ONCE("RobotBodyFilter: Transforming cloud from frame %s to %s", - tmpCloud.header.frame_id.c_str(), this->outputFrame.c_str()); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: Transforming cloud from frame %s to %s", + tmpCloud.header.frame_id.c_str(), + this->outputFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform(this->outputFrame, - tmpCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - ROS_ERROR_DELAYED_THROTTLE(3, "RobotBodyFilter: Cannot transform " + if (!this->tfBuffer->canTransform( + this->outputFrame, tmpCloud.header.frame_id, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + auto &clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE( + nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform " "point cloud to output frame. Something's wrong with TFs: %s", err.c_str()); return false; } - transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, this->outputFrame, this->channelsToTransform); + transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, + this->outputFrame, this->channelsToTransform); } return true; } -template -bool RobotBodyFilter::getShapeTransform(point_containment_filter::ShapeHandle shapeHandle, Eigen::Isometry3d &transform) const { +template +bool RobotBodyFilter::getShapeTransform( + point_containment_filter::ShapeHandle shapeHandle, + Eigen::Isometry3d &transform) const { // make sure you locked this->modelMutex - // check if the given shapeHandle has been registered to a link during addRobotMaskFromUrdf call. + // check if the given shapeHandle has been registered to a link during + // addRobotMaskFromUrdf call. if (this->shapesToLinks.find(shapeHandle) == this->shapesToLinks.end()) { - ROS_ERROR_STREAM_THROTTLE(3, "RobotBodyFilter: Invalid shape handle: " << to_string(shapeHandle)); + // RCLCPP_ERROR_THROTTLE( + // 3, "RobotBodyFilter: Invalid shape handle: " << + // to_string(shapeHandle)); return false; } - const auto& collision = this->shapesToLinks.at(shapeHandle); + const auto &collision = this->shapesToLinks.at(shapeHandle); - if (this->transformCache.find(collision.cacheKey) == this->transformCache.end()) { + if (this->transformCache.find(collision.cacheKey) == + this->transformCache.end()) { // do not log the error because shape mask would do it for us return false; } @@ -871,13 +1122,14 @@ bool RobotBodyFilter::getShapeTransform(point_containment_filter::ShapeHandle return false; } - const auto& tf1 = *this->transformCache.at(collision.cacheKey); - const auto& tf2 = *this->transformCacheAfterScan.at(collision.cacheKey); + const auto &tf1 = *this->transformCache.at(collision.cacheKey); + const auto &tf2 = *this->transformCacheAfterScan.at(collision.cacheKey); const Eigen::Quaterniond quat1(tf1.rotation().matrix()); const Eigen::Quaterniond quat2(tf1.rotation().matrix()); const auto r = this->cacheLookupBetweenScansRatio; - transform.translation() = tf1.translation() * (1 - r) + tf2.translation() * r; + transform.translation() = + tf1.translation() * (1 - r) + tf2.translation() * r; const Eigen::Quaterniond quat3 = quat1.slerp(r, quat2); transform.linear() = quat3.toRotationMatrix(); } @@ -885,17 +1137,19 @@ bool RobotBodyFilter::getShapeTransform(point_containment_filter::ShapeHandle return true; } -template -void RobotBodyFilter::updateTransformCache(const ros::Time &time, const ros::Time& afterScanTime) { +template +void RobotBodyFilter::updateTransformCache( + const rclcpp::Time &time, const rclcpp::Time &afterScanTime) { // make sure you locked this->modelMutex - // clear the cache so that maskContainment always uses only these tf data and not some older + // clear the cache so that maskContainment always uses only these tf data and + // not some older this->transformCache.clear(); - if (afterScanTime.sec != 0) + if (afterScanTime.seconds() != 0) this->transformCacheAfterScan.clear(); - // iterate over all links corresponding to some masking shape and update their cached transforms relative - // to fixed_frame + // iterate over all links corresponding to some masking shape and update their + // cached transforms relative to fixed_frame for (auto &shapeToLink : this->shapesToLinks) { const auto &collisionBody = shapeToLink.second; @@ -905,14 +1159,17 @@ void RobotBodyFilter::updateTransformCache(const ros::Time &time, const ros:: // here we assume the tf frames' names correspond to the link names const auto linkFrame = link->name; - // the collision object may have a different origin than the visual, we need to account for that - const auto &collisionOffsetTransform = urdfPose2EigenTransform(collision->origin); + // the collision object may have a different origin than the visual, we need + // to account for that + const auto &collisionOffsetTransform = + urdfPose2EigenTransform(collision->origin); { auto linkTransformTfOptional = this->tfFramesWatchdog->lookupTransform( - linkFrame, time, remainingTime(time, this->reachableTransformTimeout)); + linkFrame, time, + remainingTime(time, this->reachableTransformTimeout)); - if (!linkTransformTfOptional) // has no value + if (!linkTransformTfOptional) // has no value continue; const auto &linkTransformTf = linkTransformTfOptional.value(); @@ -921,15 +1178,16 @@ void RobotBodyFilter::updateTransformCache(const ros::Time &time, const ros:: const auto &transform = linkTransformEigen * collisionOffsetTransform; this->transformCache[collisionBody.cacheKey] = - std::allocate_shared(Eigen::aligned_allocator(), transform); + std::allocate_shared( + Eigen::aligned_allocator(), transform); } - if (afterScanTime.sec != 0) - { + if (afterScanTime.seconds() != 0) { auto linkTransformTfOptional = this->tfFramesWatchdog->lookupTransform( - linkFrame, afterScanTime, remainingTime(time, this->reachableTransformTimeout)); + linkFrame, afterScanTime, + remainingTime(time, this->reachableTransformTimeout)); - if (!linkTransformTfOptional) // has no value + if (!linkTransformTfOptional) // has no value continue; const auto &linkTransformTf = linkTransformTfOptional.value(); @@ -938,16 +1196,19 @@ void RobotBodyFilter::updateTransformCache(const ros::Time &time, const ros:: const auto &transform = linkTransformEigen * collisionOffsetTransform; this->transformCacheAfterScan[collisionBody.cacheKey] = - std::allocate_shared(Eigen::aligned_allocator(), transform); + std::allocate_shared( + Eigen::aligned_allocator(), transform); } } } -template -void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { +template +void RobotBodyFilter::addRobotMaskFromUrdf(const string &urdfModel) { if (urdfModel.empty()) { - ROS_ERROR("RobotBodyFilter: Empty string passed as robot model to addRobotMaskFromUrdf. " - "Robot body filtering is not going to work."); + RCLCPP_ERROR(nodeHandle.get_logger(), + "RobotBodyFilter: Empty string passed as robot model to " + "addRobotMaskFromUrdf. " + "Robot body filtering is not going to work."); return; } @@ -955,10 +1216,11 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { urdf::Model parsedUrdfModel; bool urdfParseSucceeded = parsedUrdfModel.initString(urdfModel); if (!urdfParseSucceeded) { - ROS_ERROR_STREAM("RobotBodyFilter: The URDF model given in parameter '" << - this->robotDescriptionParam << "' cannot be parsed. See " - "urdf::Model::initString for debugging, or try running " - "'gzsdf my_robot.urdf'"); + ROS_ERROR_STREAM("RobotBodyFilter: The URDF model given in parameter '" + << this->robotDescriptionParam + << "' cannot be parsed. See " + "urdf::Model::initString for debugging, or try running " + "'gzsdf my_robot.urdf'"); return; } @@ -973,22 +1235,27 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { // add all model's collision links as masking shapes for (const auto &links : parsedUrdfModel.links_) { - const auto& link = links.second; + const auto &link = links.second; // every link can have multiple collision elements size_t collisionIndex = 0; - for (const auto& collision : link->collision_array) { + for (const auto &collision : link->collision_array) { if (collision->geometry == nullptr) { - ROS_WARN("RobotBodyFilter: Collision element without geometry found in link %s of robot %s. " - "This collision element will not be filtered out.", - link->name.c_str(), parsedUrdfModel.getName().c_str()); - continue; // collisionIndex is intentionally not increased + RCLCPP_WARN( + nodeHandle.get_logger(), + "RobotBodyFilter: Collision element without geometry found " + "in link %s of robot %s. " + "This collision element will not be filtered out.", + link->name.c_str(), parsedUrdfModel.getName().c_str()); + continue; // collisionIndex is intentionally not increased } const auto NAME_LINK = link->name; const auto NAME_COLLISION_NAME = "*::" + collision->name; - const auto NAME_LINK_COLLISION_NR = link->name + "::" + std::to_string(collisionIndex); - const auto NAME_LINK_COLLISON_NAME = link->name + "::" + collision->name; + const auto NAME_LINK_COLLISION_NR = + link->name + "::" + std::to_string(collisionIndex); + const auto NAME_LINK_COLLISON_NAME = + link->name + "::" + collision->name; const std::vector collisionNames = { NAME_LINK, @@ -1000,14 +1267,14 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { std::set collisionNamesSet; std::set collisionNamesContains; std::set collisionNamesShadow; - for (const auto& name : collisionNames) - { + for (const auto &name : collisionNames) { collisionNamesSet.insert(name); collisionNamesContains.insert(name + CONTAINS_SUFFIX); collisionNamesShadow.insert(name + SHADOW_SUFFIX); } - // if onlyLinks is nonempty, make sure this collision belongs to a specified link + // if onlyLinks is nonempty, make sure this collision belongs to a + // specified link if (!this->onlyLinks.empty()) { if (isSetIntersectionEmpty(collisionNamesSet, this->onlyLinks)) { ++collisionIndex; @@ -1016,60 +1283,87 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { } // if the link is ignored, go on - if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredEverywhere)) { + if (!isSetIntersectionEmpty(collisionNamesSet, + this->linksIgnoredEverywhere)) { ++collisionIndex; continue; } const auto collisionShape = constructShape(*collision->geometry); - const auto shapeName = collision->name.empty() ? NAME_LINK_COLLISION_NR : NAME_LINK_COLLISON_NAME; + const auto shapeName = collision->name.empty() + ? NAME_LINK_COLLISION_NR + : NAME_LINK_COLLISON_NAME; - // if the shape could not be constructed, ignore it (e.g. mesh was not found) + // if the shape could not be constructed, ignore it (e.g. mesh was not + // found) if (collisionShape == nullptr) { - ROS_WARN("Could not construct shape for collision %s, ignoring it.", shapeName.c_str()); + RCLCPP_WARN( + nodeHandle.get_logger(), + "Could not construct shape for collision %s, ignoring it.", + shapeName.c_str()); ++collisionIndex; continue; } - // add the collision shape to shapeMask; the inflation parameters come into play here - const auto containsTestInflation = this->getLinkInflationForContainsTest(collisionNames); - const auto shadowTestInflation = this->getLinkInflationForShadowTest(collisionNames); - const auto bsphereInflation = this->getLinkInflationForBoundingSphere(collisionNames); - const auto bboxInflation = this->getLinkInflationForBoundingBox(collisionNames); - const auto shapeHandle = this->shapeMask->addShape(collisionShape, - containsTestInflation.scale, containsTestInflation.padding, shadowTestInflation.scale, - shadowTestInflation.padding, bsphereInflation.scale, bsphereInflation.padding, - bboxInflation.scale, bboxInflation.padding, false, shapeName); - this->shapesToLinks[shapeHandle.contains] = this->shapesToLinks[shapeHandle.shadow] = - this->shapesToLinks[shapeHandle.bsphere] = this->shapesToLinks[shapeHandle.bbox] = - CollisionBodyWithLink(collision, link, collisionIndex, shapeHandle); - - if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInBoundingSphere)) { + // add the collision shape to shapeMask; the inflation parameters come + // into play here + const auto containsTestInflation = + this->getLinkInflationForContainsTest(collisionNames); + const auto shadowTestInflation = + this->getLinkInflationForShadowTest(collisionNames); + const auto bsphereInflation = + this->getLinkInflationForBoundingSphere(collisionNames); + const auto bboxInflation = + this->getLinkInflationForBoundingBox(collisionNames); + const auto shapeHandle = this->shapeMask->addShape( + collisionShape, containsTestInflation.scale, + containsTestInflation.padding, shadowTestInflation.scale, + shadowTestInflation.padding, bsphereInflation.scale, + bsphereInflation.padding, bboxInflation.scale, + bboxInflation.padding, false, shapeName); + this->shapesToLinks[shapeHandle.contains] = + this->shapesToLinks[shapeHandle.shadow] = + this->shapesToLinks[shapeHandle.bsphere] = + this->shapesToLinks[shapeHandle.bbox] = + CollisionBodyWithLink(collision, link, collisionIndex, + shapeHandle); + + if (!isSetIntersectionEmpty(collisionNamesSet, + this->linksIgnoredInBoundingSphere)) { this->shapesIgnoredInBoundingSphere.insert(shapeHandle.bsphere); } - if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInBoundingBox)) { + if (!isSetIntersectionEmpty(collisionNamesSet, + this->linksIgnoredInBoundingBox)) { this->shapesIgnoredInBoundingBox.insert(shapeHandle.bbox); } - if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInContainsTest)) { + if (!isSetIntersectionEmpty(collisionNamesSet, + this->linksIgnoredInContainsTest)) { ignoreInContainsTest.insert(shapeHandle); } - if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInShadowTest)) { + if (!isSetIntersectionEmpty(collisionNamesSet, + this->linksIgnoredInShadowTest)) { ignoreInShadowTest.insert(shapeHandle); } ++collisionIndex; } - // no collision element found; only warn for links that are not ignored and have at least one visual + // no collision element found; only warn for links that are not ignored + // and have at least one visual if (collisionIndex == 0 && !link->visual_array.empty()) { - if ((this->onlyLinks.empty() || (this->onlyLinks.find(link->name) != this->onlyLinks.end())) && - this->linksIgnoredEverywhere.find(link->name) == this->linksIgnoredEverywhere.end()) { - ROS_WARN( - "RobotBodyFilter: No collision element found for link %s of robot %s. This link will not be filtered out " - "from laser scans.", link->name.c_str(), parsedUrdfModel.getName().c_str()); + if ((this->onlyLinks.empty() || + (this->onlyLinks.find(link->name) != this->onlyLinks.end())) && + this->linksIgnoredEverywhere.find(link->name) == + this->linksIgnoredEverywhere.end()) { + RCLCPP_WARN( + nodeHandle.get_logger(), + "RobotBodyFilter: No collision element found for link %s of " + "robot %s. This link will not be filtered out " + "from laser scans.", + link->name.c_str(), parsedUrdfModel.getName().c_str()); } } } @@ -1080,7 +1374,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { this->shapeMask->updateInternalShapeLists(); std::set monitoredFrames; - for (const auto& shapeToLink : this->shapesToLinks) + for (const auto &shapeToLink : this->shapesToLinks) monitoredFrames.insert(shapeToLink.second.link->name); // Issue #6: Monitor sensor frame even if it is not a part of the model if (!this->sensorFrame.empty()) @@ -1090,16 +1384,14 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const string& urdfModel) { } } -template -void RobotBodyFilter::clearRobotMask() { +template void RobotBodyFilter::clearRobotMask() { { std::lock_guard guard(*this->modelMutex); std::unordered_set removedMultiShapes; - for (const auto& shapeToLink : this->shapesToLinks) { - const auto& multiShape = shapeToLink.second.multiHandle; - if (removedMultiShapes.find(multiShape) == removedMultiShapes.end()) - { + for (const auto &shapeToLink : this->shapesToLinks) { + const auto &multiShape = shapeToLink.second.multiHandle; + if (removedMultiShapes.find(multiShape) == removedMultiShapes.end()) { this->shapeMask->removeShape(multiShape, false); removedMultiShapes.insert(multiShape); } @@ -1117,109 +1409,109 @@ void RobotBodyFilter::clearRobotMask() { } template -void RobotBodyFilter::publishDebugMarkers(const ros::Time& scanTime) const { +void RobotBodyFilter::publishDebugMarkers( + const rclcpp::Time &scanTime) const { // assume this->modelMutex is locked if (this->publishDebugContainsMarker) { - visualization_msgs::MarkerArray markerArray; - std_msgs::ColorRGBA color; + visualization_msgs::msg::MarkerArray markerArray; + std_msgs::msg::ColorRGBA color; color.g = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForContainsTest(), scanTime, - color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForContainsTest(), + scanTime, color, markerArray); this->debugContainsMarkerPublisher.publish(markerArray); } if (this->publishDebugShadowMarker) { - visualization_msgs::MarkerArray markerArray; - std_msgs::ColorRGBA color; + visualization_msgs::msg::MarkerArray markerArray; + std_msgs::msg::ColorRGBA color; color.b = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForShadowTest(), scanTime, - color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForShadowTest(), + scanTime, color, markerArray); this->debugShadowMarkerPublisher.publish(markerArray); } if (this->publishDebugBsphereMarker) { - visualization_msgs::MarkerArray markerArray; - std_msgs::ColorRGBA color; + visualization_msgs::msg::MarkerArray markerArray; + std_msgs::msg::ColorRGBA color; color.g = 1.0; color.b = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingSphere(), scanTime, - color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingSphere(), + scanTime, color, markerArray); this->debugBsphereMarkerPublisher.publish(markerArray); } if (this->publishDebugBboxMarker) { - visualization_msgs::MarkerArray markerArray; - std_msgs::ColorRGBA color; + visualization_msgs::msg::MarkerArray markerArray; + std_msgs::msg::ColorRGBA color; color.r = 1.0; color.b = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingBox(), scanTime, - color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingBox(), + scanTime, color, markerArray); this->debugBboxMarkerPublisher.publish(markerArray); } } template void RobotBodyFilter::publishDebugPointClouds( - const sensor_msgs::PointCloud2& projectedPointCloud, - const std::vector &pointMask) const -{ - if (this->publishDebugPclInside) - { - sensor_msgs::PointCloud2 insideCloud; - CREATE_FILTERED_CLOUD(projectedPointCloud, insideCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::INSIDE)); + const sensor_msgs::msg::PointCloud2 &projectedPointCloud, + const std::vector &pointMask) const { + if (this->publishDebugPclInside) { + sensor_msgs::msg::PointCloud2 insideCloud; + CREATE_FILTERED_CLOUD( + projectedPointCloud, insideCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::INSIDE)); this->debugPointCloudInsidePublisher.publish(insideCloud); } - if (this->publishDebugPclClip) - { - sensor_msgs::PointCloud2 clipCloud; - CREATE_FILTERED_CLOUD(projectedPointCloud, clipCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::CLIP)); + if (this->publishDebugPclClip) { + sensor_msgs::msg::PointCloud2 clipCloud; + CREATE_FILTERED_CLOUD( + projectedPointCloud, clipCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::CLIP)); this->debugPointCloudClipPublisher.publish(clipCloud); } - if (this->publishDebugPclShadow) - { - sensor_msgs::PointCloud2 shadowCloud; - CREATE_FILTERED_CLOUD(projectedPointCloud, shadowCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::SHADOW)); + if (this->publishDebugPclShadow) { + sensor_msgs::msg::PointCloud2 shadowCloud; + CREATE_FILTERED_CLOUD( + projectedPointCloud, shadowCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::SHADOW)); this->debugPointCloudShadowPublisher.publish(shadowCloud); } } -template +template void RobotBodyFilter::computeAndPublishBoundingSphere( - const sensor_msgs::PointCloud2& projectedPointCloud) const -{ + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { if (!this->computeBoundingSphere && !this->computeDebugBoundingSphere) return; // assume this->modelMutex is locked - // when computing bounding spheres for publication, we want to publish them to the time of the - // scan, so we need to set cacheLookupBetweenScansRatio again to zero - if (this->cacheLookupBetweenScansRatio != 0.0) - { + // when computing bounding spheres for publication, we want to publish them to + // the time of the scan, so we need to set cacheLookupBetweenScansRatio again + // to zero + if (this->cacheLookupBetweenScansRatio != 0.0) { this->cacheLookupBetweenScansRatio = 0.0; this->shapeMask->updateBodyPoses(); } - const auto& scanTime = projectedPointCloud.header.stamp; + const auto &scanTime = projectedPointCloud.header.stamp; std::vector spheres; { - visualization_msgs::MarkerArray boundingSphereDebugMsg; - for (const auto &shapeHandleAndBody : this->shapeMask->getBodiesForBoundingSphere()) - { + visualization_msgs::msg::MarkerArray boundingSphereDebugMsg; + for (const auto &shapeHandleAndBody : + this->shapeMask->getBodiesForBoundingSphere()) { const auto &shapeHandle = shapeHandleAndBody.first; const auto &body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingSphere.find(shapeHandle) != this->shapesIgnoredInBoundingSphere.end()) + if (this->shapesIgnoredInBoundingSphere.find(shapeHandle) != + this->shapesIgnoredInBoundingSphere.end()) continue; bodies::BoundingSphere sphere; @@ -1227,9 +1519,8 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( spheres.push_back(sphere); - if (this->computeDebugBoundingSphere) - { - visualization_msgs::Marker msg; + if (this->computeDebugBoundingSphere) { + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; @@ -1242,8 +1533,8 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( msg.color.g = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::SPHERE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::SPHERE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "bsphere/" + this->shapesToLinks.at(shapeHandle).cacheKey; msg.frame_locked = static_cast(true); @@ -1256,23 +1547,21 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( } } - if (this->computeBoundingSphere) - { + if (this->computeBoundingSphere) { bodies::BoundingSphere boundingSphere; bodies::mergeBoundingSpheres(spheres, boundingSphere); - robot_body_filter::SphereStamped boundingSphereMsg; - boundingSphereMsg.header.stamp = scanTime; - boundingSphereMsg.header.frame_id = this->filteringFrame; - boundingSphereMsg.sphere.radius = - static_cast(boundingSphere.radius); - boundingSphereMsg.sphere.center = tf2::toMsg(boundingSphere.center); + // robot_body_filter::SphereStamped boundingSphereMsg; + // boundingSphereMsg.header.stamp = scanTime; + // boundingSphereMsg.header.frame_id = this->filteringFrame; + // boundingSphereMsg.sphere.radius = + // static_cast(boundingSphere.radius); + // boundingSphereMsg.sphere.center = tf2::toMsg(boundingSphere.center); - this->boundingSpherePublisher.publish(boundingSphereMsg); + // this->boundingSpherePublisher.publish(boundingSphereMsg); - if (this->publishBoundingSphereMarker) - { - visualization_msgs::Marker msg; + if (this->publishBoundingSphereMarker) { + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; @@ -1285,52 +1574,53 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( msg.color.g = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::SPHERE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::SPHERE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "bounding_sphere"; msg.frame_locked = static_cast(true); this->boundingSphereMarkerPublisher.publish(msg); } - if (this->publishNoBoundingSpherePointcloud) - { - sensor_msgs::PointCloud2 noSphereCloud; - CREATE_FILTERED_CLOUD(projectedPointCloud, noSphereCloud, this->keepCloudsOrganized, - ((Eigen::Vector3d(*x_it, *y_it, *z_it)- boundingSphere.center).norm() > boundingSphere.radius)); + if (this->publishNoBoundingSpherePointcloud) { + sensor_msgs::msg::PointCloud2 noSphereCloud; + CREATE_FILTERED_CLOUD( + projectedPointCloud, noSphereCloud, this->keepCloudsOrganized, + ((Eigen::Vector3d(*x_it, *y_it, *z_it) - boundingSphere.center) + .norm() > boundingSphere.radius)); this->scanPointCloudNoBoundingSpherePublisher.publish(noSphereCloud); } } } -template +template void RobotBodyFilter::computeAndPublishBoundingBox( - const sensor_msgs::PointCloud2& projectedPointCloud) const -{ + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { if (!this->computeBoundingBox && !this->computeDebugBoundingBox) return; // assume this->modelMutex is locked - // when computing bounding boxes for publication, we want to publish them to the time of the - // scan, so we need to set cacheLookupBetweenScansRatio again to zero - if (this->cacheLookupBetweenScansRatio != 0.0) - { + // when computing bounding boxes for publication, we want to publish them to + // the time of the scan, so we need to set cacheLookupBetweenScansRatio again + // to zero + if (this->cacheLookupBetweenScansRatio != 0.0) { this->cacheLookupBetweenScansRatio = 0.0; this->shapeMask->updateBodyPoses(); } - const auto& scanTime = projectedPointCloud.header.stamp; + const auto &scanTime = projectedPointCloud.header.stamp; std::vector boxes; { - visualization_msgs::MarkerArray boundingBoxDebugMsg; - for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) - { - const auto& shapeHandle = shapeHandleAndBody.first; - const auto& body = shapeHandleAndBody.second; + visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; + for (const auto &shapeHandleAndBody : + this->shapeMask->getBodiesForBoundingBox()) { + const auto &shapeHandle = shapeHandleAndBody.first; + const auto &body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != + this->shapesIgnoredInBoundingBox.end()) continue; bodies::AxisAlignedBoundingBox box; @@ -1339,7 +1629,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox( boxes.push_back(box); if (this->computeDebugBoundingBox) { - visualization_msgs::Marker msg; + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; @@ -1350,8 +1640,8 @@ void RobotBodyFilter::computeAndPublishBoundingBox( msg.color.g = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::CUBE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "bbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; msg.frame_locked = static_cast(true); @@ -1364,13 +1654,12 @@ void RobotBodyFilter::computeAndPublishBoundingBox( } } - if (this->computeBoundingBox) - { + if (this->computeBoundingBox) { bodies::AxisAlignedBoundingBox box; bodies::mergeBoundingBoxes(boxes, box); const auto boxFloat = box.cast(); - geometry_msgs::PolygonStamped boundingBoxMsg; + geometry_msgs::msg::PolygonStamped boundingBoxMsg; boundingBoxMsg.header.stamp = scanTime; boundingBoxMsg.header.frame_id = this->filteringFrame; @@ -1381,9 +1670,8 @@ void RobotBodyFilter::computeAndPublishBoundingBox( this->boundingBoxPublisher.publish(boundingBoxMsg); - if (this->publishBoundingBoxMarker) - { - visualization_msgs::Marker msg; + if (this->publishBoundingBoxMarker) { + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; @@ -1394,8 +1682,8 @@ void RobotBodyFilter::computeAndPublishBoundingBox( msg.color.r = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::CUBE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "bounding_box"; msg.frame_locked = static_cast(true); @@ -1413,49 +1701,54 @@ void RobotBodyFilter::computeAndPublishBoundingBox( cropBox.setInputCloud(bboxCropInput); cropBox.setKeepOrganized(this->keepCloudsOrganized); - cropBox.setMin(Eigen::Vector4f(boxFloat.min()[0], boxFloat.min()[1], boxFloat.min()[2], 0.0)); - cropBox.setMax(Eigen::Vector4f(boxFloat.max()[0], boxFloat.max()[1], boxFloat.max()[2], 0.0)); + cropBox.setMin(Eigen::Vector4f(boxFloat.min()[0], boxFloat.min()[1], + boxFloat.min()[2], 0.0)); + cropBox.setMax(Eigen::Vector4f(boxFloat.max()[0], boxFloat.max()[1], + boxFloat.max()[2], 0.0)); pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::PointCloud2Ptr boxFilteredCloud(new sensor_msgs::PointCloud2()); + sensor_msgs::msg::PointCloud2Ptr boxFilteredCloud( + new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp + boxFilteredCloud->header.stamp = + scanTime; // PCL strips precision of timestamp this->scanPointCloudNoBoundingBoxPublisher.publish(boxFilteredCloud); } } } -template +template void RobotBodyFilter::computeAndPublishOrientedBoundingBox( - const sensor_msgs::PointCloud2& projectedPointCloud) const -{ - if (!this->computeOrientedBoundingBox && !this->computeDebugOrientedBoundingBox) + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { + if (!this->computeOrientedBoundingBox && + !this->computeDebugOrientedBoundingBox) return; // assume this->modelMutex is locked - // when computing bounding boxes for publication, we want to publish them to the time of the - // scan, so we need to set cacheLookupBetweenScansRatio again to zero - if (this->cacheLookupBetweenScansRatio != 0.0) - { + // when computing bounding boxes for publication, we want to publish them to + // the time of the scan, so we need to set cacheLookupBetweenScansRatio again + // to zero + if (this->cacheLookupBetweenScansRatio != 0.0) { this->cacheLookupBetweenScansRatio = 0.0; this->shapeMask->updateBodyPoses(); } - const auto& scanTime = projectedPointCloud.header.stamp; + const auto &scanTime = projectedPointCloud.header.stamp; std::vector boxes; { - visualization_msgs::MarkerArray boundingBoxDebugMsg; - for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) - { - const auto& shapeHandle = shapeHandleAndBody.first; - const auto& body = shapeHandleAndBody.second; + visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; + for (const auto &shapeHandleAndBody : + this->shapeMask->getBodiesForBoundingBox()) { + const auto &shapeHandle = shapeHandleAndBody.first; + const auto &body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != + this->shapesIgnoredInBoundingBox.end()) continue; bodies::OrientedBoundingBox box; @@ -1464,18 +1757,20 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( boxes.push_back(box); if (this->computeDebugOrientedBoundingBox) { - visualization_msgs::Marker msg; + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; tf2::toMsg(box.getExtents(), msg.scale); - msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); - msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + msg.pose.position = + tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); + msg.pose.orientation = + tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); msg.color.g = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::CUBE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "obbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; msg.frame_locked = static_cast(true); @@ -1484,13 +1779,14 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( } if (this->computeDebugOrientedBoundingBox) { - this->orientedBoundingBoxDebugMarkerPublisher.publish(boundingBoxDebugMsg); + this->orientedBoundingBoxDebugMarkerPublisher.publish( + boundingBoxDebugMsg); } } - if (this->computeOrientedBoundingBox) - { - bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); + if (this->computeOrientedBoundingBox) { + bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), + Eigen::Vector3d::Zero()); bodies::mergeBoundingBoxesApprox(boxes, box); robot_body_filter::OrientedBoundingBoxStamped boundingBoxMsg; @@ -1499,25 +1795,28 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( boundingBoxMsg.header.frame_id = this->filteringFrame; tf2::toMsg(box.getExtents(), boundingBoxMsg.obb.extents); - tf2::toMsg(box.getPose().translation(), boundingBoxMsg.obb.pose.translation); - boundingBoxMsg.obb.pose.rotation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + tf2::toMsg(box.getPose().translation(), + boundingBoxMsg.obb.pose.translation); + boundingBoxMsg.obb.pose.rotation = + tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); this->orientedBoundingBoxPublisher.publish(boundingBoxMsg); - if (this->publishOrientedBoundingBoxMarker) - { - visualization_msgs::Marker msg; + if (this->publishOrientedBoundingBoxMarker) { + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; tf2::toMsg(box.getExtents(), msg.scale); - msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); - msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + msg.pose.position = + tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); + msg.pose.orientation = + tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); msg.color.r = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::CUBE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "oriented_bounding_box"; msg.frame_locked = static_cast(true); @@ -1536,66 +1835,72 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( cropBox.setKeepOrganized(this->keepCloudsOrganized); const auto e = box.getExtents(); - cropBox.setMin(Eigen::Vector4f(-e.x()/2, -e.y()/2, -e.z()/2, 0.0)); - cropBox.setMax(Eigen::Vector4f(e.x()/2, e.y()/2, e.z()/2, 0.0)); + cropBox.setMin(Eigen::Vector4f(-e.x() / 2, -e.y() / 2, -e.z() / 2, 0.0)); + cropBox.setMax(Eigen::Vector4f(e.x() / 2, e.y() / 2, e.z() / 2, 0.0)); cropBox.setTranslation(box.getPose().translation().cast()); - cropBox.setRotation(box.getPose().linear().eulerAngles(0, 1, 2).cast()); + cropBox.setRotation( + box.getPose().linear().eulerAngles(0, 1, 2).cast()); pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::PointCloud2Ptr boxFilteredCloud(new sensor_msgs::PointCloud2()); + sensor_msgs::msg::PointCloud2Ptr boxFilteredCloud( + new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp + boxFilteredCloud->header.stamp = + scanTime; // PCL strips precision of timestamp - this->scanPointCloudNoOrientedBoundingBoxPublisher.publish(boxFilteredCloud); + this->scanPointCloudNoOrientedBoundingBoxPublisher.publish( + boxFilteredCloud); } } } -template +template void RobotBodyFilter::computeAndPublishLocalBoundingBox( - const sensor_msgs::PointCloud2& projectedPointCloud) const -{ + const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { if (!this->computeLocalBoundingBox && !this->computeDebugLocalBoundingBox) return; // assume this->modelMutex is locked - const auto& scanTime = projectedPointCloud.header.stamp; + const auto &scanTime = projectedPointCloud.header.stamp; std::string err; try { - if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, - this->filteringFrame, - scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), - &err)) { - ROS_ERROR_DELAYED_THROTTLE(3.0, "Cannot get transform %s->%s. Error is %s.", - this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), err.c_str()); + if (!this->tfBuffer->canTransform( + this->localBoundingBoxFrame, this->filteringFrame, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + RCLCPP_ERROR_DELAYED_THROTTLE(nodeHandle.get_logger(), 3.0, + "Cannot get transform %s->%s. Error is %s.", + this->filteringFrame.c_str(), + this->localBoundingBoxFrame.c_str(), + err.c_str()); return; } - } catch (tf2::TransformException& e) { - ROS_ERROR_DELAYED_THROTTLE(3.0, "Cannot get transform %s->%s. Error is %s.", - this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), e.what()); + } catch (tf2::TransformException &e) { + RCLCPP_ERROR_DELAYED_THROTTLE(nodeHandle.get_logger(), 3.0, + "Cannot get transform %s->%s. Error is %s.", + this->filteringFrame.c_str(), + this->localBoundingBoxFrame.c_str(), + e.what()); return; } - const auto localTfMsg = this->tfBuffer->lookupTransform(this->localBoundingBoxFrame, - this->filteringFrame, scanTime); + const auto localTfMsg = this->tfBuffer->lookupTransform( + this->localBoundingBoxFrame, this->filteringFrame, scanTime); const Eigen::Isometry3d localTf = tf2::transformToEigen(localTfMsg.transform); std::vector boxes; { - visualization_msgs::MarkerArray boundingBoxDebugMsg; - for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) - { - const auto& shapeHandle = shapeHandleAndBody.first; - const auto& body = shapeHandleAndBody.second; + visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; + for (const auto &shapeHandleAndBody : + this->shapeMask->getBodiesForBoundingBox()) { + const auto &shapeHandle = shapeHandleAndBody.first; + const auto &body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != + this->shapesIgnoredInBoundingBox.end()) continue; bodies::AxisAlignedBoundingBox box; @@ -1604,7 +1909,7 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( boxes.push_back(box); if (this->computeDebugLocalBoundingBox) { - visualization_msgs::Marker msg; + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->localBoundingBoxFrame; @@ -1614,8 +1919,8 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( msg.color.g = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::CUBE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "lbbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; msg.frame_locked = static_cast(true); @@ -1628,12 +1933,11 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( } } - if (this->computeLocalBoundingBox) - { + if (this->computeLocalBoundingBox) { bodies::AxisAlignedBoundingBox box; bodies::mergeBoundingBoxes(boxes, box); - geometry_msgs::PolygonStamped boundingBoxMsg; + geometry_msgs::msg::PolygonStamped boundingBoxMsg; boundingBoxMsg.header.stamp = scanTime; boundingBoxMsg.header.frame_id = this->localBoundingBoxFrame; @@ -1644,9 +1948,8 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( this->localBoundingBoxPublisher.publish(boundingBoxMsg); - if (this->publishLocalBoundingBoxMarker) - { - visualization_msgs::Marker msg; + if (this->publishLocalBoundingBoxMarker) { + visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->localBoundingBoxFrame; @@ -1656,8 +1959,8 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( msg.color.r = 1.0; msg.color.a = 0.5; - msg.type = visualization_msgs::Marker::CUBE; - msg.action = visualization_msgs::Marker::ADD; + msg.type = visualization_msgs::msg::Marker::CUBE; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = "local_bounding_box"; msg.frame_locked = static_cast(true); @@ -1675,51 +1978,55 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( cropBox.setInputCloud(bboxCropInput); cropBox.setKeepOrganized(this->keepCloudsOrganized); - cropBox.setMin(Eigen::Vector4f(box.min()[0], box.min()[1], box.min()[2], 0.0)); - cropBox.setMax(Eigen::Vector4f(box.max()[0], box.max()[1], box.max()[2], 0.0)); + cropBox.setMin( + Eigen::Vector4f(box.min()[0], box.min()[1], box.min()[2], 0.0)); + cropBox.setMax( + Eigen::Vector4f(box.max()[0], box.max()[1], box.max()[2], 0.0)); const Eigen::Isometry3d localTfInv = localTf.inverse(); cropBox.setTranslation(localTfInv.translation().cast()); - cropBox.setRotation(localTfInv.linear().eulerAngles(0, 1, 2).cast()); + cropBox.setRotation( + localTfInv.linear().eulerAngles(0, 1, 2).cast()); pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::PointCloud2Ptr boxFilteredCloud(new sensor_msgs::PointCloud2()); + sensor_msgs::msg::PointCloud2Ptr boxFilteredCloud( + new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp + boxFilteredCloud->header.stamp = + scanTime; // PCL strips precision of timestamp this->scanPointCloudNoLocalBoundingBoxPublisher.publish(boxFilteredCloud); } } } -template +template void RobotBodyFilter::createBodyVisualizationMsg( - const std::map& bodies, - const ros::Time& stamp, const std_msgs::ColorRGBA& color, - visualization_msgs::MarkerArray& markerArray) const -{ - // when computing the markers for publication, we want to publish them to the time of the - // scan, so we need to set cacheLookupBetweenScansRatio again to zero - if (this->cacheLookupBetweenScansRatio != 0.0) - { + const std::map + &bodies, + const rclcpp::Time &stamp, const std_msgs::ColorRGBA &color, + visualization_msgs::msg::MarkerArray &markerArray) const { + // when computing the markers for publication, we want to publish them to the + // time of the scan, so we need to set cacheLookupBetweenScansRatio again to + // zero + if (this->cacheLookupBetweenScansRatio != 0.0) { this->cacheLookupBetweenScansRatio = 0.0; this->shapeMask->updateBodyPoses(); } - for (const auto &shapeHandleAndBody : bodies) - { + for (const auto &shapeHandleAndBody : bodies) { const auto &shapeHandle = shapeHandleAndBody.first; auto body = shapeHandleAndBody.second; - visualization_msgs::Marker msg; + visualization_msgs::msg::Marker msg; bodies::constructMarkerFromBody(body, msg); msg.header.stamp = stamp; msg.header.frame_id = this->filteringFrame; msg.color = color; - msg.action = visualization_msgs::Marker::ADD; + msg.action = visualization_msgs::msg::Marker::ADD; msg.ns = this->shapesToLinks.at(shapeHandle).cacheKey; msg.frame_locked = static_cast(true); @@ -1727,8 +2034,9 @@ void RobotBodyFilter::createBodyVisualizationMsg( } } -template -void RobotBodyFilter::robotDescriptionUpdated(dynamic_reconfigure::ConfigConstPtr newConfig) { +template +void RobotBodyFilter::robotDescriptionUpdated( + dynamic_reconfigure::ConfigConstPtr newConfig) { auto robotDescriptionIdx = static_cast(-1); for (size_t i = 0; i < newConfig->strs.size(); ++i) { if (newConfig->strs[i].name == this->robotDescriptionUpdatesFieldName) { @@ -1737,13 +2045,16 @@ void RobotBodyFilter::robotDescriptionUpdated(dynamic_reconfigure::ConfigCons } } - // robot_description parameter was not found, so we don't have to restart the filter + // robot_description parameter was not found, so we don't have to restart the + // filter if (robotDescriptionIdx == static_cast(-1)) return; auto urdf = newConfig->strs[robotDescriptionIdx].value; - ROS_INFO("RobotBodyFilter: Reloading robot model because of dynamic_reconfigure update. Filter operation stopped."); + RCLCPP_INFO(nodeHandle.get_logger(), + "RobotBodyFilter: Reloading robot model because of " + "dynamic_reconfigure update. Filter operation stopped."); this->tfFramesWatchdog->pause(); this->configured_ = false; @@ -1752,27 +2063,30 @@ void RobotBodyFilter::robotDescriptionUpdated(dynamic_reconfigure::ConfigCons this->addRobotMaskFromUrdf(urdf); this->tfFramesWatchdog->unpause(); - this->timeConfigured = ros::Time::now(); + this->timeConfigured = rclcpp::Time::now(); this->configured_ = true; - ROS_INFO("RobotBodyFilter: Robot model reloaded, resuming filter operation."); + RCLCPP_INFO( + nodeHandle.get_logger(), + "RobotBodyFilter: Robot model reloaded, resuming filter operation."); } -template +template bool RobotBodyFilter::triggerModelReload(std_srvs::TriggerRequest &, - std_srvs::TriggerResponse &) -{ + std_srvs::TriggerResponse &) { std::string urdf; auto success = this->nodeHandle.getParam(this->robotDescriptionParam, urdf); - if (!success) - { - ROS_ERROR_STREAM("RobotBodyFilter: Parameter " << this->robotDescriptionParam - << " doesn't exist."); + if (!success) { + ROS_ERROR_STREAM("RobotBodyFilter: Parameter " + << this->robotDescriptionParam << " doesn't exist."); return false; } - ROS_INFO("RobotBodyFilter: Reloading robot model because of trigger. Filter operation stopped."); + RCLCPP_INFO( + nodeHandle.get_logger(), + "RobotBodyFilter: Reloading robot model because of trigger. Filter " + "operation stopped."); this->tfFramesWatchdog->pause(); this->configured_ = false; @@ -1781,88 +2095,80 @@ bool RobotBodyFilter::triggerModelReload(std_srvs::TriggerRequest &, this->addRobotMaskFromUrdf(urdf); this->tfFramesWatchdog->unpause(); - this->timeConfigured = ros::Time::now(); + this->timeConfigured = rclcpp::Time::now(); this->configured_ = true; - ROS_INFO("RobotBodyFilter: Robot model reloaded, resuming filter operation."); + RCLCPP_INFO( + nodeHandle.get_logger(), + "RobotBodyFilter: Robot model reloaded, resuming filter operation."); return true; } -template -RobotBodyFilter::~RobotBodyFilter(){ +template RobotBodyFilter::~RobotBodyFilter() { if (this->tfFramesWatchdog != nullptr) this->tfFramesWatchdog->stop(); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest( - const string &linkName) const -{ + const string &linkName) const { return this->getLinkInflationForContainsTest({linkName}); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest( - const std::vector& linkNames) const -{ + const std::vector &linkNames) const { return this->getLinkInflation(linkNames, this->defaultContainsInflation, - this->perLinkContainsInflation); + this->perLinkContainsInflation); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest( - const string &linkName) const -{ + const string &linkName) const { return this->getLinkInflationForShadowTest({linkName}); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest( - const std::vector& linkNames) const -{ + const std::vector &linkNames) const { return this->getLinkInflation(linkNames, this->defaultShadowInflation, - this->perLinkShadowInflation); + this->perLinkShadowInflation); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere( - const string &linkName) const -{ + const string &linkName) const { return this->getLinkInflationForBoundingSphere({linkName}); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere( - const std::vector& linkNames) const -{ + const std::vector &linkNames) const { return this->getLinkInflation(linkNames, this->defaultBsphereInflation, - this->perLinkBsphereInflation); + this->perLinkBsphereInflation); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox( - const string &linkName) const -{ + const string &linkName) const { return this->getLinkInflationForBoundingBox({linkName}); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox( - const std::vector& linkNames) const -{ + const std::vector &linkNames) const { return this->getLinkInflation(linkNames, this->defaultBboxInflation, - this->perLinkBboxInflation); + this->perLinkBboxInflation); } -template +template ScaleAndPadding RobotBodyFilter::getLinkInflation( - const std::vector& linkNames, const ScaleAndPadding& defaultInflation, - const std::map& perLinkInflation) const -{ + const std::vector &linkNames, + const ScaleAndPadding &defaultInflation, + const std::map &perLinkInflation) const { ScaleAndPadding result = defaultInflation; - for (const auto& linkName : linkNames) - { + for (const auto &linkName : linkNames) { if (perLinkInflation.find(linkName) != perLinkInflation.end()) result = perLinkInflation.at(linkName); } @@ -1871,21 +2177,19 @@ ScaleAndPadding RobotBodyFilter::getLinkInflation( } ScaleAndPadding::ScaleAndPadding(double scale, double padding) - :scale(scale), padding(padding) -{ -} + : scale(scale), padding(padding) {} -bool ScaleAndPadding::operator==(const ScaleAndPadding& other) const -{ +bool ScaleAndPadding::operator==(const ScaleAndPadding &other) const { return this->scale == other.scale && this->padding == other.padding; } -bool ScaleAndPadding::operator!=(const ScaleAndPadding& other) const -{ +bool ScaleAndPadding::operator!=(const ScaleAndPadding &other) const { return !(*this == other); } -} +} // namespace robot_body_filter -PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, filters::FilterBase) -PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, + filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, + filters::FilterBase) diff --git a/src/utils/tf2_eigen.cpp b/src/utils/tf2_eigen.cpp index a49fa21..76f1b00 100644 --- a/src/utils/tf2_eigen.cpp +++ b/src/utils/tf2_eigen.cpp @@ -1,7 +1,7 @@ #include namespace tf2 { -void toMsg(const Eigen::Vector3d& in, geometry_msgs::Point32& out) +void toMsg(const Eigen::Vector3d& in, geometry_msgs::msg::Point32& out) { out.x = in.x(); out.y = in.y(); diff --git a/src/utils/tf2_sensor_msgs.cpp b/src/utils/tf2_sensor_msgs.cpp index 01e2702..6c887a1 100644 --- a/src/utils/tf2_sensor_msgs.cpp +++ b/src/utils/tf2_sensor_msgs.cpp @@ -32,7 +32,7 @@ bool fieldNameMatchesChannel(const std::string& fieldName, const std::string& ch } } -void transformChannel(const sensor_msgs::PointCloud2& cloudIn, sensor_msgs::PointCloud2& cloudOut, +void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs::msg::PointCloud2& cloudOut, const Eigen::Isometry3f& t, const std::string& channelPrefix, const CloudChannelType type) { if (num_points(cloudIn) == 0) @@ -74,21 +74,21 @@ void transformChannel(const sensor_msgs::PointCloud2& cloudIn, sensor_msgs::Poin } } -void transformChannel(sensor_msgs::PointCloud2& cloud, const geometry_msgs::Transform& tf, +void transformChannel(sensor_msgs::msg::PointCloud2& cloud, const geometry_msgs::msg::Transform& tf, const std::string& channelPrefix, CloudChannelType type) { const auto t = tf2::transformToEigen(tf).cast(); transformChannel(cloud, cloud, t, channelPrefix, type); } -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf) +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf) { return transformWithChannels(in, out, tf, DEFAULT_CHANNELS); } -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf, +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf, const std::unordered_map& channels) { std::unordered_set channelsPresent; @@ -114,8 +114,8 @@ sensor_msgs::PointCloud2& transformWithChannels( return out; } -sensor_msgs::PointCloud2& transformOnlyChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, const geometry_msgs::TransformStamped& tf, +sensor_msgs::msg::PointCloud2& transformOnlyChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, const geometry_msgs::msg::TransformStamped& tf, const std::unordered_map& channels) { std::unordered_set channelsPresent; @@ -157,20 +157,20 @@ sensor_msgs::PointCloud2& transformOnlyChannels( return out; } -sensor_msgs::PointCloud2& transformOnlyXYZ(const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, - const geometry_msgs::TransformStamped& tf) { +sensor_msgs::msg::PointCloud2& transformOnlyXYZ(const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, + const geometry_msgs::msg::TransformStamped& tf) { return transformOnlyChannels(in, out, tf, XYZ_CHANNELS); } -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, tf2_ros::Buffer& tfBuffer, const std::string& targetFrame) { return transformWithChannels(in, out, tfBuffer, targetFrame, DEFAULT_CHANNELS); } -sensor_msgs::PointCloud2& transformWithChannels( - const sensor_msgs::PointCloud2& in, sensor_msgs::PointCloud2& out, +sensor_msgs::msg::PointCloud2& transformWithChannels( + const sensor_msgs::msg::PointCloud2& in, sensor_msgs::msg::PointCloud2& out, tf2_ros::Buffer& tfBuffer, const std::string& targetFrame, const std::unordered_map& channels) { From 097764588f6e596818931afc343242d0ec7d36d8 Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 16 Apr 2024 16:59:27 -0400 Subject: [PATCH 06/54] in progress --- CMakeLists.txt | 9 + include/robot_body_filter/RobotBodyFilter.h | 23 +-- include/robot_body_filter/utils/bodies.h | 22 ++- .../robot_body_filter/utils/filter_utils.hpp | 6 +- include/robot_body_filter/utils/obb.hpp | 150 ++++++++++++++ msg/OrientedBoundingBoxStamped.msg | 2 +- msg/SphereStamped.msg | 2 +- src/RobotBodyFilter.cpp | 11 +- src/utils/obb.cpp | 183 ++++++++++++++++++ 9 files changed, 373 insertions(+), 35 deletions(-) create mode 100644 include/robot_body_filter/utils/obb.hpp create mode 100644 src/utils/obb.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 430a7c7..8e38c40 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS std_msgs tf2 tf2_ros + tf2_msgs urdf visualization_msgs pcl_conversions @@ -73,5 +74,13 @@ target_include_directories(${PROJECT_NAME}_filter ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/OrientedBoundingBox.msg" + "msg/OrientedBoundingBoxStamped.msg" + DEPENDENCIES builtin_interfaces std_msgs geometry_msgs std_srvs tf2_msgs tf2_sensor_msgs tf2_ros tf2_eigen visualization_msgs + ) + ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() \ No newline at end of file diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 8a3e01b..dd5b17b 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -1,16 +1,6 @@ #ifndef ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ #define ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ -#include "robot_body_filter/utils/tf2_sensor_msgs.h" -#include -#include -#include -#include -#include -#include -#include -#include - #include #include @@ -19,7 +9,8 @@ #include // #include // #include -// #include +#include +#include // #include // #include // #include @@ -27,16 +18,16 @@ // #include // #include #include -// #include -// #include +#include +#include #include // #include #include // #include -// #include -// #include +#include +#include #include -#include +// #include #include #include #include diff --git a/include/robot_body_filter/utils/bodies.h b/include/robot_body_filter/utils/bodies.h index f02ab8b..1bee703 100644 --- a/include/robot_body_filter/utils/bodies.h +++ b/include/robot_body_filter/utils/bodies.h @@ -1,26 +1,30 @@ #ifndef ROBOT_BODY_FILTER_BODIES_H #define ROBOT_BODY_FILTER_BODIES_H -#include +#include "obb.hpp" #include +#include #if __has_include() #include #else -// #error not found. Please, update geometric_shapes library to version 0.6.5+ or 0.7.4+ +// #error not found. Please, update geometric_shapes +// library to version 0.6.5+ or 0.7.4+ #endif // #include -namespace bodies -{ +namespace bodies { typedef bodies::AABB AxisAlignedBoundingBox; -// typedef bodies::OBB OrientedBoundingBox; +typedef bodies::OBB OrientedBoundingBox; -/** \brief Compute AABB for the body at different pose. Can't use setPose() because we want `body` to be const. */ -void computeBoundingBoxAt(const bodies::Body* body, AxisAlignedBoundingBox& bbox, const Eigen::Isometry3d& pose); +/** \brief Compute AABB for the body at different pose. Can't use setPose() + * because we want `body` to be const. */ +void computeBoundingBoxAt(const bodies::Body *body, + AxisAlignedBoundingBox &bbox, + const Eigen::Isometry3d &pose); -} +} // namespace bodies -#endif //ROBOT_BODY_FILTER_BODIES_H +#endif // ROBOT_BODY_FILTER_BODIES_H diff --git a/include/robot_body_filter/utils/filter_utils.hpp b/include/robot_body_filter/utils/filter_utils.hpp index 93db161..0a05fb7 100644 --- a/include/robot_body_filter/utils/filter_utils.hpp +++ b/include/robot_body_filter/utils/filter_utils.hpp @@ -3,11 +3,11 @@ // #include // #include -#include +// #include -#include +// #include -#include "robot_body_filter/utils/string_utils.hpp" +// #include "robot_body_filter/utils/string_utils.hpp" // #include "robot_body_filter/utils/xmlrpc_traits.h" namespace robot_body_filter diff --git a/include/robot_body_filter/utils/obb.hpp b/include/robot_body_filter/utils/obb.hpp new file mode 100644 index 0000000..5cd92e4 --- /dev/null +++ b/include/robot_body_filter/utils/obb.hpp @@ -0,0 +1,150 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Open Robotics + * 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 Open Robotics 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: Martin Pecka */ + +#ifndef GEOMETRIC_SHAPES_OBB_H +#define GEOMETRIC_SHAPES_OBB_H + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace bodies { +class OBBPrivate; + +/** \brief Represents an oriented bounding box. */ +class OBB { +public: + /** \brief Initialize an oriented bounding box at position 0, with 0 extents + * and identity orientation. */ + OBB(); + OBB(const OBB &other); + OBB(const Eigen::Isometry3d &pose, const Eigen::Vector3d &extents); + virtual ~OBB(); + + OBB &operator=(const OBB &other); + + /** + * \brief Set both the pose and extents of the OBB. + * \param [in] pose New pose of the OBB. + * \param [in] extents New extents of the OBB. + */ + void setPoseAndExtents(const Eigen::Isometry3d &pose, + const Eigen::Vector3d &extents); + + /** + * \brief Get the extents of the OBB. + * \return The extents. + */ + Eigen::Vector3d getExtents() const; + + /** + * \brief Get the extents of the OBB. + * \param extents [out] The extents. + */ + void getExtents(Eigen::Vector3d &extents) const; + + /** + * \brief Get the pose of the OBB. + * \return The pose. + */ + Eigen::Isometry3d getPose() const; + + /** + * \brief Get The pose of the OBB. + * \param pose The pose. + */ + void getPose(Eigen::Isometry3d &pose) const; + + /** + * \brief Convert this OBB to an axis-aligned BB. + * \return The AABB. + */ + AABB toAABB() const; + + /** + * \brief Convert this OBB to an axis-aligned BB. + * \param aabb The AABB. + */ + void toAABB(AABB &aabb) const; + + /** + * \brief Add the other OBB to this one and compute an approximate enclosing + * OBB. \param box The other box to add. \return Pointer to this OBB after the + * update. + */ + OBB *extendApprox(const OBB &box); + + /** + * \brief Check if this OBB contains the given point. + * \param point The point to check. + * \return Whether the point is inside or not. + */ + bool contains(const Eigen::Vector3d &point) const; + + /** + * \brief Check whether this and the given OBBs have nonempty intersection. + * \param other The other OBB to check. + * \return Whether the OBBs overlap. + */ + bool overlaps(const OBB &other) const; + + /** + * \brief Check if this OBB contains whole other OBB. + * \param point The point to check. + * \return Whether the point is inside or not. + */ + bool contains(const OBB &obb) const; + + /** + * \brief Compute coordinates of the 8 vertices of this OBB. + * \return The vertices. + */ + EigenSTL::vector_Vector3d computeVertices() const; + +protected: + /** \brief PIMPL pointer */ + std::unique_ptr obb_; +}; +} // namespace bodies + +#endif // GEOMETRIC_SHAPES_OBB_H \ No newline at end of file diff --git a/msg/OrientedBoundingBoxStamped.msg b/msg/OrientedBoundingBoxStamped.msg index 18c4b63..a49b782 100644 --- a/msg/OrientedBoundingBoxStamped.msg +++ b/msg/OrientedBoundingBoxStamped.msg @@ -1,2 +1,2 @@ -Header header +std_msgs/Header header robot_body_filter/OrientedBoundingBox obb \ No newline at end of file diff --git a/msg/SphereStamped.msg b/msg/SphereStamped.msg index b06a12b..1695421 100644 --- a/msg/SphereStamped.msg +++ b/msg/SphereStamped.msg @@ -1,2 +1,2 @@ -Header header +std_msgs/Header header robot_body_filter/Sphere sphere \ No newline at end of file diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 6f3ff67..0c90a49 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -7,6 +7,7 @@ #include #include "robot_body_filter/RobotBodyFilter.h" +#include "robot_body_filter/utils/obb.hpp" // #include "pluginlib/class_list_macros.h" @@ -29,7 +30,7 @@ #include #include #include -// #include +#include #include #include @@ -876,8 +877,8 @@ bool RobotBodyFilterLaserScan::update( { // remove invalid points const float INVALID_POINT_VALUE = std::numeric_limits::quiet_NaN(); try { - sensor_msgs::msg::PointCloud2Iterator indexIt(projectedPointCloud, - "index"); + sensor_msgs::PointCloud2Iterator indexIt(projectedPointCloud, + "index"); size_t indexInScan; for (const auto maskValue : pointMask) { @@ -1203,7 +1204,7 @@ void RobotBodyFilter::updateTransformCache( } template -void RobotBodyFilter::addRobotMaskFromUrdf(const string &urdfModel) { +void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { if (urdfModel.empty()) { RCLCPP_ERROR(nodeHandle.get_logger(), "RobotBodyFilter: Empty string passed as robot model to " @@ -1709,7 +1710,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox( pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2Ptr boxFilteredCloud( + sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud( new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = diff --git a/src/utils/obb.cpp b/src/utils/obb.cpp new file mode 100644 index 0000000..13653d4 --- /dev/null +++ b/src/utils/obb.cpp @@ -0,0 +1,183 @@ +#include + +#include + +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 +#include +typedef fcl::Vec3f FCL_Vec3; +typedef fcl::OBB FCL_OBB; +#else +#include +typedef fcl::Vector3d FCL_Vec3; +typedef fcl::OBB FCL_OBB; +#endif + +namespace bodies { +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 +inline FCL_Vec3 toFcl(const Eigen::Vector3d &eigenVec) { + FCL_Vec3 result; + Eigen::Map(result.data.vs, 3, 1) = eigenVec; + return result; +} +#else +#define toFcl +#endif + +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 +Eigen::Vector3d fromFcl(const FCL_Vec3 &fclVec) { + return Eigen::Map(fclVec.data.vs, 3, 1); +} +#else +#define fromFcl +#endif + +class OBBPrivate : public FCL_OBB { +public: + using FCL_OBB::OBB; +}; + +OBB::OBB() { + obb_.reset(new OBBPrivate); + // Initialize the OBB to position 0, with 0 extents and identity rotation +#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5 + // FCL 0.6+ does not zero-initialize the OBB. + obb_->extent.setZero(); + obb_->To.setZero(); + obb_->axis.setIdentity(); +#else + // FCL 0.5 zero-initializes the OBB, so we just put the identity into + // orientation. + obb_->axis[0][0] = 1.0; + obb_->axis[1][1] = 1.0; + obb_->axis[2][2] = 1.0; +#endif +} + +OBB::OBB(const OBB &other) { obb_.reset(new OBBPrivate(*other.obb_)); } + +OBB::OBB(const Eigen::Isometry3d &pose, const Eigen::Vector3d &extents) { + obb_.reset(new OBBPrivate); + setPoseAndExtents(pose, extents); +} + +OBB &OBB::operator=(const OBB &other) { + *obb_ = *other.obb_; + return *this; +} + +void OBB::setPoseAndExtents(const Eigen::Isometry3d &pose, + const Eigen::Vector3d &extents) { + const auto rotation = pose.linear(); + +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 + obb_->axis[0] = toFcl(rotation.col(0)); + obb_->axis[1] = toFcl(rotation.col(1)); + obb_->axis[2] = toFcl(rotation.col(2)); +#else + obb_->axis = rotation; +#endif + + obb_->To = toFcl(pose.translation()); + + obb_->extent = {extents[0] / 2.0, extents[1] / 2.0, extents[2] / 2.0}; +} + +void OBB::getExtents(Eigen::Vector3d &extents) const { + extents = 2 * fromFcl(obb_->extent); +} + +Eigen::Vector3d OBB::getExtents() const { + Eigen::Vector3d extents; + getExtents(extents); + return extents; +} + +void OBB::getPose(Eigen::Isometry3d &pose) const { + pose = Eigen::Isometry3d::Identity(); + pose.translation() = fromFcl(obb_->To); +#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 + pose.linear().col(0) = fromFcl(obb_->axis[0]); + pose.linear().col(1) = fromFcl(obb_->axis[1]); + pose.linear().col(2) = fromFcl(obb_->axis[2]); +#else + pose.linear() = obb_->axis; +#endif +} + +Eigen::Isometry3d OBB::getPose() const { + Eigen::Isometry3d pose; + getPose(pose); + return pose; +} + +AABB OBB::toAABB() const { + AABB result; + toAABB(result); + return result; +} + +void OBB::toAABB(AABB &aabb) const { + aabb.extendWithTransformedBox(getPose(), getExtents()); +} + +OBB *OBB::extendApprox(const OBB &box) { + if (this->getExtents() == Eigen::Vector3d::Zero()) { + *obb_ = *box.obb_; + return this; + } + + if (this->contains(box)) + return this; + + if (box.contains(*this)) { + *obb_ = *box.obb_; + return this; + } + + *this->obb_ += *box.obb_; + return this; +} + +bool OBB::contains(const Eigen::Vector3d &point) const { + return obb_->contain(toFcl(point)); +} + +bool OBB::overlaps(const bodies::OBB &other) const { + return obb_->overlap(*other.obb_); +} + +EigenSTL::vector_Vector3d OBB::computeVertices() const { + const Eigen::Vector3d e = + getExtents() / 2; // do not use auto type, Eigen would be inefficient + // clang-format off + EigenSTL::vector_Vector3d result = { + { -e[0], -e[1], -e[2] }, + { -e[0], -e[1], e[2] }, + { -e[0], e[1], -e[2] }, + { -e[0], e[1], e[2] }, + { e[0], -e[1], -e[2] }, + { e[0], -e[1], e[2] }, + { e[0], e[1], -e[2] }, + { e[0], e[1], e[2] }, + }; + // clang-format on + + const auto pose = getPose(); + for (auto &v : result) { + v = pose * v; + } + + return result; +} + +bool OBB::contains(const OBB &obb) const { + for (const auto &v : obb.computeVertices()) { + if (!contains(v)) + return false; + } + return true; +} + +OBB::~OBB() = default; + +} // namespace bodies \ No newline at end of file From d78de542bd95102e54851ab9e5c8d7dd1a22348e Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 16 Apr 2024 18:33:28 -0400 Subject: [PATCH 07/54] builds --- include/robot_body_filter/RobotBodyFilter.h | 33 +- .../robot_body_filter/utils/filter_utils.hpp | 683 +++++++++--------- include/robot_body_filter/utils/obb.hpp | 2 - .../robot_body_filter/utils/string_utils.hpp | 2 +- src/RobotBodyFilter.cpp | 274 +++---- 5 files changed, 488 insertions(+), 506 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index dd5b17b..e6b6387 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -24,10 +25,10 @@ // #include #include // #include -#include +// #include #include #include -// #include +#include #include #include #include @@ -230,37 +231,37 @@ template class RobotBodyFilter : public filters::FilterBase { rclcpp::Publisher::SharedPtr boundingSpherePublisher; // //! Publisher of robot bounding box (relative to fixed frame). - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr boundingBoxPublisher; // //! Publisher of robot bounding box (relative to fixed frame). rclcpp::Publisher::SharedPtr orientedBoundingBoxPublisher; // //! Publisher of robot bounding box (relative to defined local frame). - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr localBoundingBoxPublisher; // //! Publisher of the bounding sphere marker. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr boundingSphereMarkerPublisher; // //! Publisher of the bounding box marker. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr boundingBoxMarkerPublisher; // //! Publisher of the oriented bounding box marker. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr orientedBoundingBoxMarkerPublisher; //! Publisher of the local bounding box marker. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr localBoundingBoxMarkerPublisher; //! Publisher of the debug bounding box markers. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr boundingBoxDebugMarkerPublisher; //! Publisher of the debug oriented bounding box markers. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr orientedBoundingBoxDebugMarkerPublisher; //! Publisher of the debug local bounding box markers. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr localBoundingBoxDebugMarkerPublisher; //! Publisher of the debug bounding sphere markers. - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr boundingSphereDebugMarkerPublisher; //! Publisher of scan_point_cloud with robot bounding box cut out. @@ -281,13 +282,13 @@ template class RobotBodyFilter : public filters::FilterBase { debugPointCloudClipPublisher; rclcpp::Publisher::SharedPtr debugPointCloudShadowPublisher; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debugContainsMarkerPublisher; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debugShadowMarkerPublisher; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debugBsphereMarkerPublisher; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr debugBboxMarkerPublisher; // ! Service server for reloading robot model. diff --git a/include/robot_body_filter/utils/filter_utils.hpp b/include/robot_body_filter/utils/filter_utils.hpp index 0a05fb7..30dbd13 100644 --- a/include/robot_body_filter/utils/filter_utils.hpp +++ b/include/robot_body_filter/utils/filter_utils.hpp @@ -10,380 +10,357 @@ // #include "robot_body_filter/utils/string_utils.hpp" // #include "robot_body_filter/utils/xmlrpc_traits.h" -namespace robot_body_filter -{ +#include +namespace robot_body_filter { -namespace -{ +namespace { /** - * \brief Internal use only. This class exposes the XmlRpcValue -> typed data conversion - * provided by filters::FilterBase::getParam(), which is normally protected. We use this - * workaround to avoid copy-pasting the code here. + * \brief Internal use only. This class exposes the XmlRpcValue -> typed data + * conversion provided by filters::FilterBase::getParam(), which is normally + * protected. We use this workaround to avoid copy-pasting the code here. */ -// template -// class FilterParamHelper : public filters::FilterBase -// { -// public: -// FilterParamHelper(const std::string& key, const XmlRpc::XmlRpcValue& value) -// { -// this->params_[key] = value[key]; -// } -// template bool getParamHelper(const std::string& name, T& value) const -// { -// return filters::FilterBase::getParam(name, value); -// } -// bool update(const F& data_in, F& data_out) override {return false;} -// protected: -// bool configure() override {return false;} -// }; -// } +template class FilterParamHelper : public filters::FilterBase { +public: + FilterParamHelper(const std::string &key, const XmlRpc::XmlRpcValue &value) { + this->params_[key] = value[key]; + } + template + bool getParamHelper(const std::string &name, T &value) const { + return filters::FilterBase::getParam(name, value); + } + bool update(const F &data_in, F &data_out) override { return false; } -// template -// class FilterBase : public filters::FilterBase -// { +protected: + bool configure() override { return false; } +}; +} // namespace -// protected: +template class FilterBase : public filters::FilterBase { -// /** \brief Type of function that converts anything to a string. */ -// template using ToStringFn = std::string (*)(const T&); +protected: + /** \brief Type of function that converts anything to a string. */ + template using ToStringFn = std::string (*)(const T &); -// /** -// * \brief Get the value of the given filter parameter, falling back to the -// * specified default value, and print out a ROS info/warning message with -// * the loaded values. -// * \tparam T Param type. -// * \param name Name of the parameter. If the name contains slashes and the full name is not found, -// * a "recursive" search is tried using the parts of the name separated by slashes. -// * This is useful if the filter config isn't loaded via a filterchain config, but via -// * a dict loaded directly to ROS parameter server. -// * \param defaultValue The default value to use. -// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the -// * messages more informative. -// * \param defaultUsed Whether the default value was used. -// * \param valueToStringFn Function that converts valid/default values to string (for console -// * logging). Set to nullptr to disable logging. -// * \return The loaded param value. -// */ -// template -// T getParamVerbose(const std::string &name, const T &defaultValue = T(), -// const std::string &unit = "", bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// T value; -// if (filters::FilterBase::getParam(name, value)) -// { -// if (valueToStringFn != nullptr) -// { -// ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << -// ", value: " << valueToStringFn(value) << -// prependIfNonEmpty(unit, " ")); -// } -// if (defaultUsed != nullptr) -// *defaultUsed = false; -// return value; -// } -// else if (this->params_.find(name) != this->params_.end()) -// { // the parameter was found, but has a wrong type -// ROS_ERROR_STREAM(this->getName() << ": Parameter " << name << " found, " -// "but its value has a wrong type. Expected XmlRpc type " << -// XmlRpcTraits::stringType << ", got type: " << -// to_string(this->params_.at(name).getType()) << -// ". Using the default value instead."); -// } -// else if (name.length() > 1 && name.find_first_of('/', 1) != std::string::npos) -// { // The parameter has slashes in its name, so try a "recursive" search -// auto slashPos = name.find_first_of('/', 1); -// auto head = name.substr(0, slashPos); -// auto tail = name.substr(slashPos + 1); -// XmlRpc::XmlRpcValue val; + /** + * \brief Get the value of the given filter parameter, falling back to the + * specified default value, and print out a ROS info/warning message + * with the loaded values. \tparam T Param type. \param name Name of the + * parameter. If the name contains slashes and the full name is not found, a + * "recursive" search is tried using the parts of the name separated by + * slashes. This is useful if the filter config isn't loaded via a filterchain + * config, but via a dict loaded directly to ROS parameter server. \param + * defaultValue The default value to use. \param unit Optional string serving + * as a [physical/SI] unit of the parameter, just to make the messages more + * informative. \param defaultUsed Whether the default value was used. \param + * valueToStringFn Function that converts valid/default values to string (for + * console logging). Set to nullptr to disable logging. \return The loaded + * param value. + */ + template + T getParamVerbose(const std::string &name, const T &defaultValue = T(), + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + T value; + if (filters::FilterBase::getParam(name, value)) { + if (valueToStringFn != nullptr) { + ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name + << ", value: " << valueToStringFn(value) + << prependIfNonEmpty(unit, " ")); + } + if (defaultUsed != nullptr) + *defaultUsed = false; + return value; + } else if (this->params_.find(name) != + this->params_ + .end()) { // the parameter was found, but has a wrong type + ROS_ERROR_STREAM( + this->getName() + << ": Parameter " << name + << " found, " + "but its value has a wrong type. Expected XmlRpc type " + << XmlRpcTraits::stringType + << ", got type: " << to_string(this->params_.at(name).getType()) + << ". Using the default value instead."); + } else if (name.length() > 1 && + name.find_first_of('/', 1) != + std::string::npos) { // The parameter has slashes in its + // name, so try a "recursive" search + auto slashPos = name.find_first_of('/', 1); + auto head = name.substr(0, slashPos); + auto tail = name.substr(slashPos + 1); + XmlRpc::XmlRpcValue val; -// if (filters::FilterBase::getParam(head, val)) -// { -// while (val.getType() == XmlRpc::XmlRpcValue::TypeStruct) -// { -// if (val.hasMember(tail)) -// { -// auto tmpFilter = FilterParamHelper(tail, val); -// if (tmpFilter.getParamHelper(tail, value)) -// { -// if (defaultUsed != nullptr) -// *defaultUsed = false; -// if (valueToStringFn != nullptr) -// { -// ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << -// ", value: " << valueToStringFn(value) << prependIfNonEmpty(unit, " ")); -// } -// return value; -// } -// else -// { -// ROS_ERROR_STREAM(this->getName() << ": Parameter " << name << " found, " -// "but its value has a wrong type. Expected XmlRpc type " << -// XmlRpcTraits::stringType << ", got type: " << -// to_string(val[tail].getType()) << -// ". Using the default value instead."); -// break; -// } -// } else { -// slashPos = tail.find_first_of('/', 1); -// if (slashPos == std::string::npos) -// break; -// head = tail.substr(0, slashPos); -// tail = tail.substr(slashPos + 1); -// if (!val.hasMember(head)) -// break; -// XmlRpc::XmlRpcValue tmp = val[head]; // tmp copy is required, otherwise mem corruption -// val = tmp; -// if (!val.valid()) -// break; -// } -// } -// } -// } + if (filters::FilterBase::getParam(head, val)) { + while (val.getType() == XmlRpc::XmlRpcValue::TypeStruct) { + if (val.hasMember(tail)) { + auto tmpFilter = FilterParamHelper(tail, val); + if (tmpFilter.getParamHelper(tail, value)) { + if (defaultUsed != nullptr) + *defaultUsed = false; + if (valueToStringFn != nullptr) { + ROS_INFO_STREAM(this->getName() + << ": Found parameter: " << name + << ", value: " << valueToStringFn(value) + << prependIfNonEmpty(unit, " ")); + } + return value; + } else { + ROS_ERROR_STREAM( + this->getName() + << ": Parameter " << name + << " found, " + "but its value has a wrong type. Expected XmlRpc type " + << XmlRpcTraits::stringType + << ", got type: " << to_string(val[tail].getType()) + << ". Using the default value instead."); + break; + } + } else { + slashPos = tail.find_first_of('/', 1); + if (slashPos == std::string::npos) + break; + head = tail.substr(0, slashPos); + tail = tail.substr(slashPos + 1); + if (!val.hasMember(head)) + break; + XmlRpc::XmlRpcValue tmp = + val[head]; // tmp copy is required, otherwise mem corruption + val = tmp; + if (!val.valid()) + break; + } + } + } + } -// if (valueToStringFn != nullptr) -// { -// ROS_INFO_STREAM(this->getName() << ": Parameter " << name -// << " not defined, assigning default: " -// << valueToStringFn(defaultValue) -// << prependIfNonEmpty(unit, " ")); -// } -// if (defaultUsed != nullptr) -// *defaultUsed = true; -// return defaultValue; -// } + if (valueToStringFn != nullptr) { + ROS_INFO_STREAM(this->getName() << ": Parameter " << name + << " not defined, assigning default: " + << valueToStringFn(defaultValue) + << prependIfNonEmpty(unit, " ")); + } + if (defaultUsed != nullptr) + *defaultUsed = true; + return defaultValue; + } -// /** \brief Get the value of the given filter parameter, falling back to the -// * specified default value, and print out a ROS info/warning message with -// * the loaded values. -// * \param name Name of the parameter. If the name contains slashes and the full name is not found, -// * a "recursive" search is tried using the parts of the name separated by slashes. -// * This is useful if the filter config isn't loaded via a filterchain config, but via -// * a dict loaded directly to ROS parameter server. -// * \param defaultValue The default value to use. -// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the -// * messages more informative. -// * \return The loaded param value. -// */ -// std::string getParamVerbose(const std::string &name, const char* defaultValue, -// const std::string &unit = "", bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// return this->getParamVerbose(name, std::string(defaultValue), unit, defaultUsed, valueToStringFn); -// } + /** \brief Get the value of the given filter parameter, falling back to the + * specified default value, and print out a ROS info/warning message + * with the loaded values. \param name Name of the parameter. If the name + * contains slashes and the full name is not found, a "recursive" search is + * tried using the parts of the name separated by slashes. This is useful if + * the filter config isn't loaded via a filterchain config, but via a dict + * loaded directly to ROS parameter server. \param defaultValue The default + * value to use. \param unit Optional string serving as a [physical/SI] unit + * of the parameter, just to make the messages more informative. \return The + * loaded param value. + */ + std::string + getParamVerbose(const std::string &name, const char *defaultValue, + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + return this->getParamVerbose(name, std::string(defaultValue), unit, + defaultUsed, valueToStringFn); + } + // getParam specializations for unsigned values + /** \brief Get the value of the given filter parameter, falling back to the + * specified default value, and print out a ROS info/warning message + * with the loaded values. \param name Name of the parameter. If the name + * contains slashes and the full name is not found, a "recursive" search is + * tried using the parts of the name separated by slashes. This is useful if + * the filter config isn't loaded via a filterchain config, but via a dict + * loaded directly to ROS parameter server. \param defaultValue The default + * value to use. \param unit Optional string serving as a [physical/SI] unit + * of the parameter, just to make the messages more informative. \return The + * loaded param value. \throw std::invalid_argument If the loaded value is + * negative. + */ + uint64_t getParamVerbose(const std::string &name, + const uint64_t &defaultValue, + const std::string &unit = "", + bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + return this->getParamUnsigned(name, defaultValue, unit, + defaultUsed, valueToStringFn); + } -// // getParam specializations for unsigned values + // there actually is an unsigned int implementation of FilterBase::getParam, + // but it doesn't tell you when the passed value is negative - instead it just + // returns false + /** \brief Get the value of the given filter parameter, falling back to the + * specified default value, and print out a ROS info/warning message + * with the loaded values. \param name Name of the parameter. If the name + * contains slashes and the full name is not found, a "recursive" search is + * tried using the parts of the name separated by slashes. This is useful if + * the filter config isn't loaded via a filterchain config, but via a dict + * loaded directly to ROS parameter server. \param defaultValue The default + * value to use. \param unit Optional string serving as a [physical/SI] unit + * of the parameter, just to make the messages more informative. \return The + * loaded param value. \throw std::invalid_argument If the loaded value is + * negative. + */ + unsigned int + getParamVerbose(const std::string &name, const unsigned int &defaultValue, + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + return this->getParamUnsigned( + name, defaultValue, unit, defaultUsed, valueToStringFn); + } -// /** \brief Get the value of the given filter parameter, falling back to the -// * specified default value, and print out a ROS info/warning message with -// * the loaded values. -// * \param name Name of the parameter. If the name contains slashes and the full name is not found, -// * a "recursive" search is tried using the parts of the name separated by slashes. -// * This is useful if the filter config isn't loaded via a filterchain config, but via -// * a dict loaded directly to ROS parameter server. -// * \param defaultValue The default value to use. -// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the -// * messages more informative. -// * \return The loaded param value. -// * \throw std::invalid_argument If the loaded value is negative. -// */ -// uint64_t getParamVerbose(const std::string &name, const uint64_t &defaultValue, -// const std::string &unit = "", bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// return this->getParamUnsigned(name, defaultValue, unit, defaultUsed, -// valueToStringFn); -// } + // ROS types specializations -// // there actually is an unsigned int implementation of FilterBase::getParam, -// // but it doesn't tell you when the passed value is negative - instead it just -// // returns false -// /** \brief Get the value of the given filter parameter, falling back to the -// * specified default value, and print out a ROS info/warning message with -// * the loaded values. -// * \param name Name of the parameter. If the name contains slashes and the full name is not found, -// * a "recursive" search is tried using the parts of the name separated by slashes. -// * This is useful if the filter config isn't loaded via a filterchain config, but via -// * a dict loaded directly to ROS parameter server. -// * \param defaultValue The default value to use. -// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the -// * messages more informative. -// * \return The loaded param value. -// * \throw std::invalid_argument If the loaded value is negative. -// */ -// unsigned int getParamVerbose(const std::string &name, -// const unsigned int &defaultValue, -// const std::string &unit = "", -// bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// return this->getParamUnsigned(name, defaultValue, unit, defaultUsed, -// valueToStringFn); -// } + /** \brief Get the value of the given filter parameter, falling back to the + * specified default value, and print out a ROS info/warning message + * with the loaded values. \param name Name of the parameter. If the name + * contains slashes and the full name is not found, a "recursive" search is + * tried using the parts of the name separated by slashes. This is useful if + * the filter config isn't loaded via a filterchain config, but via a dict + * loaded directly to ROS parameter server. \param defaultValue The default + * value to use. \param unit Optional string serving as a [physical/SI] unit + * of the parameter, just to make the messages more informative. \return The + * loaded param value. + */ + ros::Duration + getParamVerbose(const std::string &name, const ros::Duration &defaultValue, + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + return this->getParamCast( + name, defaultValue.toSec(), unit, defaultUsed, valueToStringFn); + } -// // ROS types specializations + /** \brief Get the value of the given filter parameter as a set of strings, + * falling back to the specified default value, and print out a ROS + * info/warning message with the loaded values. \tparam T Type of the values + * in the set. Only std::string and double are supported. \param name Name of + * the parameter. If the name contains slashes and the full name is not found, + * a "recursive" search is tried using the parts of the name + * separated by slashes. This is useful if the filter config isn't loaded via + * a filterchain config, but via a dict loaded directly to ROS parameter + * server. \param defaultValue The default value to use. \param unit Optional + * string serving as a [physical/SI] unit of the parameter, just to make the + * messages more informative. + * \return The loaded param value. + */ + template + std::set getParamVerboseSet( + const std::string &name, const std::set &defaultValue = std::set(), + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn> valueToStringFn = &to_string) const { + std::vector vector(defaultValue.begin(), defaultValue.end()); + vector = + this->getParamVerbose(name, vector, unit, defaultUsed, valueToStringFn); + return std::set(vector.begin(), vector.end()); + } -// /** \brief Get the value of the given filter parameter, falling back to the -// * specified default value, and print out a ROS info/warning message with -// * the loaded values. -// * \param name Name of the parameter. If the name contains slashes and the full name is not found, -// * a "recursive" search is tried using the parts of the name separated by slashes. -// * This is useful if the filter config isn't loaded via a filterchain config, but via -// * a dict loaded directly to ROS parameter server. -// * \param defaultValue The default value to use. -// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the -// * messages more informative. -// * \return The loaded param value. -// */ -// ros::Duration getParamVerbose(const std::string &name, -// const ros::Duration &defaultValue, -// const std::string &unit = "", -// bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// return this->getParamCast(name, defaultValue.toSec(), unit, defaultUsed, -// valueToStringFn); -// } + template > + MapType getParamVerboseMap( + const std::string &name, + const std::map &defaultValue = std::map(), + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + // convert default value to XmlRpc so that we can utilize + // FilterBase::getParam(XmlRpcValue). + XmlRpc::XmlRpcValue defaultValueXmlRpc; + defaultValueXmlRpc + .begin(); // calls assertStruct() which mutates this value into a struct + for (const auto &val : defaultValue) + defaultValueXmlRpc[val.first] = val.second; -// /** \brief Get the value of the given filter parameter as a set of strings, falling back to the -// * specified default value, and print out a ROS info/warning message with -// * the loaded values. -// * \tparam T Type of the values in the set. Only std::string and double are supported. -// * \param name Name of the parameter. If the name contains slashes and the full name is not found, -// * a "recursive" search is tried using the parts of the name separated by slashes. -// * This is useful if the filter config isn't loaded via a filterchain config, but via -// * a dict loaded directly to ROS parameter server. -// * \param defaultValue The default value to use. -// * \param unit Optional string serving as a [physical/SI] unit of the parameter, just to make the -// * messages more informative. -// * \return The loaded param value. -// */ -// template -// std::set getParamVerboseSet( -// const std::string &name, -// const std::set &defaultValue = std::set(), -// const std::string &unit = "", -// bool* defaultUsed = nullptr, -// ToStringFn> valueToStringFn = &to_string) const -// { -// std::vector vector(defaultValue.begin(), defaultValue.end()); -// vector = this->getParamVerbose(name, vector, unit, defaultUsed, valueToStringFn); -// return std::set(vector.begin(), vector.end()); -// } + // get the param value as a XmlRpcValue + bool innerDefaultUsed; + auto valueXmlRpc = + this->getParamVerbose(name, defaultValueXmlRpc, unit, &innerDefaultUsed, + (ToStringFn)nullptr); -// template> -// MapType getParamVerboseMap( -// const std::string &name, -// const std::map &defaultValue = std::map(), -// const std::string &unit = "", -// bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// // convert default value to XmlRpc so that we can utilize FilterBase::getParam(XmlRpcValue). -// XmlRpc::XmlRpcValue defaultValueXmlRpc; -// defaultValueXmlRpc.begin(); // calls assertStruct() which mutates this value into a struct -// for (const auto& val : defaultValue) -// defaultValueXmlRpc[val.first] = val.second; + // convert to map + MapType value; + bool hasWrongTypeItems = false; + for (auto &pairXmlRpc : valueXmlRpc) { + if (pairXmlRpc.second.getType() == XmlRpcTraits::xmlRpcType) { + value[pairXmlRpc.first] = pairXmlRpc.second.operator T &(); + } else if (XmlRpcTraits::xmlRpcType == + XmlRpc::XmlRpcValue::TypeDouble && + pairXmlRpc.second.getType() == XmlRpc::XmlRpcValue::TypeInt) { + // special handling of the case when doubles are expected but an int is + // provided + value[pairXmlRpc.first] = + static_cast(pairXmlRpc.second.operator int &()); + } else { + ROS_WARN_STREAM( + this->getName() + << ": Invalid value for dict parameter " << name << " key " + << pairXmlRpc.first << ". Expected XmlRpc type " + << XmlRpcTraits::stringType << ", got type: " + << to_string(pairXmlRpc.second.getType()) << ". Skipping value."); + hasWrongTypeItems = true; + } + } -// // get the param value as a XmlRpcValue -// bool innerDefaultUsed; -// auto valueXmlRpc = this->getParamVerbose(name, defaultValueXmlRpc, unit, &innerDefaultUsed, -// (ToStringFn)nullptr); + if (value.empty() && hasWrongTypeItems) { + value = defaultValue; + if (defaultUsed != nullptr) + *defaultUsed = true; + if (valueToStringFn != nullptr) { + ROS_ERROR_STREAM( + this->getName() + << ": Dict parameter " << name + << " got only invalid types of values, assigning default: " + << valueToStringFn(defaultValue) << prependIfNonEmpty(unit, " ")); + } + } else { + if (defaultUsed != nullptr) + *defaultUsed = innerDefaultUsed; + if (valueToStringFn != nullptr) { + if (innerDefaultUsed) { + ROS_INFO_STREAM(this->getName() << ": Parameter " << name + << " not defined, assigning default: " + << valueToStringFn(defaultValue) + << prependIfNonEmpty(unit, " ")); + } else { + ROS_INFO_STREAM(this->getName() + << ": Found parameter: " << name + << ", value: " << valueToStringFn(value) + << prependIfNonEmpty(unit, " ")); + } + } + } -// // convert to map -// MapType value; -// bool hasWrongTypeItems = false; -// for (auto& pairXmlRpc : valueXmlRpc) -// { -// if (pairXmlRpc.second.getType() == XmlRpcTraits::xmlRpcType) -// { -// value[pairXmlRpc.first] = pairXmlRpc.second.operator T&(); -// } -// else if (XmlRpcTraits::xmlRpcType == XmlRpc::XmlRpcValue::TypeDouble && pairXmlRpc.second.getType() == XmlRpc::XmlRpcValue::TypeInt) -// { -// // special handling of the case when doubles are expected but an int is provided -// value[pairXmlRpc.first] = static_cast(pairXmlRpc.second.operator int&()); -// } -// else -// { -// ROS_WARN_STREAM(this->getName() << ": Invalid value for dict parameter " << name -// << " key " << pairXmlRpc.first << ". Expected XmlRpc type " << XmlRpcTraits::stringType -// << ", got type: " << to_string(pairXmlRpc.second.getType()) << ". Skipping value."); -// hasWrongTypeItems = true; -// } -// } + return value; + } -// if (value.empty() && hasWrongTypeItems) -// { -// value = defaultValue; -// if (defaultUsed != nullptr) -// *defaultUsed = true; -// if (valueToStringFn != nullptr) -// { -// ROS_ERROR_STREAM(this->getName() << ": Dict parameter " << name -// << " got only invalid types of values, assigning default: " -// << valueToStringFn(defaultValue) -// << prependIfNonEmpty(unit, " ")); -// } -// } else { -// if (defaultUsed != nullptr) -// *defaultUsed = innerDefaultUsed; -// if (valueToStringFn != nullptr) -// { -// if (innerDefaultUsed) -// { -// ROS_INFO_STREAM(this->getName() << ": Parameter " << name -// << " not defined, assigning default: " -// << valueToStringFn(defaultValue) -// << prependIfNonEmpty(unit, " ")); -// } -// else -// { -// ROS_INFO_STREAM(this->getName() << ": Found parameter: " << name << -// ", value: " << valueToStringFn(value) << -// prependIfNonEmpty(unit, " ")); -// } -// } -// } +private: + template + Result + getParamUnsigned(const std::string &name, const Result &defaultValue, + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + const Param signedValue = + this->getParamVerbose(name, static_cast(defaultValue), unit, + defaultUsed, valueToStringFn); + if (signedValue < 0) { + if (valueToStringFn != nullptr) { + ROS_ERROR_STREAM(this->getName() + << ": Value " << valueToStringFn(signedValue) + << " of unsigned parameter " << name + << " is negative."); + } + throw std::invalid_argument(name); + } + return static_cast(signedValue); + } -// return value; -// } + // generic casting getParam() + template + Result getParamCast(const std::string &name, const Param &defaultValue, + const std::string &unit = "", bool *defaultUsed = nullptr, + ToStringFn valueToStringFn = &to_string) const { + const Param paramValue = this->getParamVerbose( + name, defaultValue, unit, defaultUsed, valueToStringFn); + return Result(paramValue); + } +}; -// private: - -// template -// Result getParamUnsigned(const std::string &name, const Result &defaultValue, -// const std::string &unit = "", bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// const Param signedValue = this->getParamVerbose(name, static_cast(defaultValue), unit, -// defaultUsed, valueToStringFn); -// if (signedValue < 0) -// { -// if (valueToStringFn != nullptr) -// { -// ROS_ERROR_STREAM(this->getName() << ": Value " << valueToStringFn(signedValue) << -// " of unsigned parameter " << name << " is negative."); -// } -// throw std::invalid_argument(name); -// } -// return static_cast(signedValue); -// } - -// // generic casting getParam() -// template -// Result getParamCast(const std::string &name, const Param &defaultValue, -// const std::string &unit = "", bool* defaultUsed = nullptr, -// ToStringFn valueToStringFn = &to_string) const -// { -// const Param paramValue = this->getParamVerbose(name, defaultValue, unit, defaultUsed, -// valueToStringFn); -// return Result(paramValue); -// } - -// }; - -} -#endif //ROBOT_BODY_FILTER_UTILS_FILTER_UTILS_HPP +} // namespace robot_body_filter +#endif // ROBOT_BODY_FILTER_UTILS_FILTER_UTILS_HPP diff --git a/include/robot_body_filter/utils/obb.hpp b/include/robot_body_filter/utils/obb.hpp index 5cd92e4..30748ef 100644 --- a/include/robot_body_filter/utils/obb.hpp +++ b/include/robot_body_filter/utils/obb.hpp @@ -37,8 +37,6 @@ #ifndef GEOMETRIC_SHAPES_OBB_H #define GEOMETRIC_SHAPES_OBB_H -#include -#include #include #include diff --git a/include/robot_body_filter/utils/string_utils.hpp b/include/robot_body_filter/utils/string_utils.hpp index 76bc8fd..2001f55 100644 --- a/include/robot_body_filter/utils/string_utils.hpp +++ b/include/robot_body_filter/utils/string_utils.hpp @@ -84,7 +84,7 @@ std::string removeSuffix(const std::string& str, const std::string& suffix, bool template inline std::string to_string(const T &value) { - return std::to_string(value); + return to_string(value); } template<> diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 0c90a49..bb0fd0c 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1,13 +1,9 @@ #include +#include #include #include -#include -#include -#include - #include "robot_body_filter/RobotBodyFilter.h" -#include "robot_body_filter/utils/obb.hpp" // #include "pluginlib/class_list_macros.h" @@ -48,9 +44,10 @@ RobotBodyFilter::RobotBodyFilter() : privateNodeHandle("~") { template bool RobotBodyFilter::configure() { // this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", // rclcpp::Duration(60.0), "s"); - if (this->tfBuffer == nullptr) { - this->tfBuffer = std::make_shared(this->tfBufferLength); + tf2::Duration tf2_duration = tf2_ros::fromRclcpp(this->tfBufferLength); + this->tfBuffer = + std::make_shared(nodeHandle.get_clock(), tf2_duration); this->tfListener = std::make_unique(*this->tfBuffer); } else { @@ -452,7 +449,7 @@ template bool RobotBodyFilter::configure() { // { // initialize the robot body to be masked out - // string robotUrdf; + // std::string robotUrdf; // while (!this->nodeHandle.getParam(this->robotDescriptionParam, robotUrdf) // || robotUrdf.length() == 0) { // if (this->failWithoutRobotDescription) @@ -609,12 +606,12 @@ bool RobotBodyFilter::computeMask( rclcpp::Time(scanTime) + rclcpp::Duration::from_seconds(scanDuration)); size_t updateBodyPosesEvery; - if (this->modelPoseUpdateInterval.sec == 0 && - this->modelPoseUpdateInterval.nsec == 0) { + if (this->modelPoseUpdateInterval.seconds() == 0 && + this->modelPoseUpdateInterval.nanoseconds() == 0) { updateBodyPosesEvery = 1; } else { updateBodyPosesEvery = static_cast( - ceil(this->modelPoseUpdateInterval.toSec() / scanDuration * + ceil(this->modelPoseUpdateInterval.seconds() / scanDuration * num_points(projectedPointCloud))); // prevent division by zero if (updateBodyPosesEvery == 0) @@ -1421,7 +1418,7 @@ void RobotBodyFilter::publishDebugMarkers( color.a = 0.5; createBodyVisualizationMsg(this->shapeMask->getBodiesForContainsTest(), scanTime, color, markerArray); - this->debugContainsMarkerPublisher.publish(markerArray); + this->debugContainsMarkerPublisher->publish(markerArray); } if (this->publishDebugShadowMarker) { @@ -1431,7 +1428,7 @@ void RobotBodyFilter::publishDebugMarkers( color.a = 0.5; createBodyVisualizationMsg(this->shapeMask->getBodiesForShadowTest(), scanTime, color, markerArray); - this->debugShadowMarkerPublisher.publish(markerArray); + this->debugShadowMarkerPublisher->publish(markerArray); } if (this->publishDebugBsphereMarker) { @@ -1442,7 +1439,7 @@ void RobotBodyFilter::publishDebugMarkers( color.a = 0.5; createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingSphere(), scanTime, color, markerArray); - this->debugBsphereMarkerPublisher.publish(markerArray); + this->debugBsphereMarkerPublisher->publish(markerArray); } if (this->publishDebugBboxMarker) { @@ -1453,7 +1450,7 @@ void RobotBodyFilter::publishDebugMarkers( color.a = 0.5; createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingBox(), scanTime, color, markerArray); - this->debugBboxMarkerPublisher.publish(markerArray); + this->debugBboxMarkerPublisher->publish(markerArray); } } @@ -1466,7 +1463,7 @@ void RobotBodyFilter::publishDebugPointClouds( CREATE_FILTERED_CLOUD( projectedPointCloud, insideCloud, this->keepCloudsOrganized, (pointMask[i] == RayCastingShapeMask::MaskValue::INSIDE)); - this->debugPointCloudInsidePublisher.publish(insideCloud); + this->debugPointCloudInsidePublisher->publish(insideCloud); } if (this->publishDebugPclClip) { @@ -1474,7 +1471,7 @@ void RobotBodyFilter::publishDebugPointClouds( CREATE_FILTERED_CLOUD( projectedPointCloud, clipCloud, this->keepCloudsOrganized, (pointMask[i] == RayCastingShapeMask::MaskValue::CLIP)); - this->debugPointCloudClipPublisher.publish(clipCloud); + this->debugPointCloudClipPublisher->publish(clipCloud); } if (this->publishDebugPclShadow) { @@ -1482,7 +1479,7 @@ void RobotBodyFilter::publishDebugPointClouds( CREATE_FILTERED_CLOUD( projectedPointCloud, shadowCloud, this->keepCloudsOrganized, (pointMask[i] == RayCastingShapeMask::MaskValue::SHADOW)); - this->debugPointCloudShadowPublisher.publish(shadowCloud); + this->debugPointCloudShadowPublisher->publish(shadowCloud); } } @@ -1544,7 +1541,7 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( } if (this->computeDebugBoundingSphere) { - this->boundingSphereDebugMarkerPublisher.publish(boundingSphereDebugMsg); + this->boundingSphereDebugMarkerPublisher->publish(boundingSphereDebugMsg); } } @@ -1580,7 +1577,7 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( msg.ns = "bounding_sphere"; msg.frame_locked = static_cast(true); - this->boundingSphereMarkerPublisher.publish(msg); + this->boundingSphereMarkerPublisher->publish(msg); } if (this->publishNoBoundingSpherePointcloud) { @@ -1589,7 +1586,7 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( projectedPointCloud, noSphereCloud, this->keepCloudsOrganized, ((Eigen::Vector3d(*x_it, *y_it, *z_it) - boundingSphere.center) .norm() > boundingSphere.radius)); - this->scanPointCloudNoBoundingSpherePublisher.publish(noSphereCloud); + this->scanPointCloudNoBoundingSpherePublisher->publish(noSphereCloud); } } } @@ -1651,7 +1648,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox( } if (this->computeDebugBoundingBox) { - this->boundingBoxDebugMarkerPublisher.publish(boundingBoxDebugMsg); + this->boundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); } } @@ -1669,7 +1666,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox( tf2::toMsg(box.min(), boundingBoxMsg.polygon.points[0]); tf2::toMsg(box.max(), boundingBoxMsg.polygon.points[1]); - this->boundingBoxPublisher.publish(boundingBoxMsg); + this->boundingBoxPublisher->publish(boundingBoxMsg); if (this->publishBoundingBoxMarker) { visualization_msgs::msg::Marker msg; @@ -1688,7 +1685,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox( msg.ns = "bounding_box"; msg.frame_locked = static_cast(true); - this->boundingBoxMarkerPublisher.publish(msg); + this->boundingBoxMarkerPublisher->publish(msg); } // compute and publish the scan_point_cloud with robot bounding box removed @@ -1716,7 +1713,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox( boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - this->scanPointCloudNoBoundingBoxPublisher.publish(boxFilteredCloud); + this->scanPointCloudNoBoundingBoxPublisher->publish(*boxFilteredCloud); } } } @@ -1753,7 +1750,10 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( continue; bodies::OrientedBoundingBox box; - body->computeBoundingBox(box); + // TODO: This is probabbly wrong, originally computeBoundingBox took in + // the OrientedBoundingBox directly but I can't get that to work + auto aaba = box.toAABB(); + body->computeBoundingBox(aaba); boxes.push_back(box); @@ -1780,7 +1780,7 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( } if (this->computeDebugOrientedBoundingBox) { - this->orientedBoundingBoxDebugMarkerPublisher.publish( + this->orientedBoundingBoxDebugMarkerPublisher->publish( boundingBoxDebugMsg); } } @@ -1788,20 +1788,21 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( if (this->computeOrientedBoundingBox) { bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); - bodies::mergeBoundingBoxesApprox(boxes, box); + // TODO: fix this + // bodies::mergeBoundingBoxesApprox(boxes, box); - robot_body_filter::OrientedBoundingBoxStamped boundingBoxMsg; + // robot_body_filter::OrientedBoundingBoxStamped boundingBoxMsg; - boundingBoxMsg.header.stamp = scanTime; - boundingBoxMsg.header.frame_id = this->filteringFrame; + // boundingBoxMsg.header.stamp = scanTime; + // boundingBoxMsg.header.frame_id = this->filteringFrame; - tf2::toMsg(box.getExtents(), boundingBoxMsg.obb.extents); - tf2::toMsg(box.getPose().translation(), - boundingBoxMsg.obb.pose.translation); - boundingBoxMsg.obb.pose.rotation = - tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + // tf2::toMsg(box.getExtents(), boundingBoxMsg.obb.extents); + // tf2::toMsg(box.getPose().translation(), + // boundingBoxMsg.obb.pose.translation); + // boundingBoxMsg.obb.pose.rotation = + // tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); - this->orientedBoundingBoxPublisher.publish(boundingBoxMsg); + // this->orientedBoundingBoxPublisher.publish(boundingBoxMsg); if (this->publishOrientedBoundingBoxMarker) { visualization_msgs::msg::Marker msg; @@ -1821,7 +1822,7 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( msg.ns = "oriented_bounding_box"; msg.frame_locked = static_cast(true); - this->orientedBoundingBoxMarkerPublisher.publish(msg); + this->orientedBoundingBoxMarkerPublisher->publish(msg); } // compute and publish the scan_point_cloud with robot bounding box removed @@ -1845,14 +1846,14 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2Ptr boxFilteredCloud( + sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud( new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - this->scanPointCloudNoOrientedBoundingBoxPublisher.publish( - boxFilteredCloud); + this->scanPointCloudNoOrientedBoundingBoxPublisher->publish( + *boxFilteredCloud); } } } @@ -1871,19 +1872,17 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( if (!this->tfBuffer->canTransform( this->localBoundingBoxFrame, this->filteringFrame, scanTime, remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - RCLCPP_ERROR_DELAYED_THROTTLE(nodeHandle.get_logger(), 3.0, - "Cannot get transform %s->%s. Error is %s.", - this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), - err.c_str()); + RCLCPP_ERROR(nodeHandle.get_logger(), + "Cannot get transform %s->%s. Error is %s.", + this->filteringFrame.c_str(), + this->localBoundingBoxFrame.c_str(), err.c_str()); return; } } catch (tf2::TransformException &e) { - RCLCPP_ERROR_DELAYED_THROTTLE(nodeHandle.get_logger(), 3.0, - "Cannot get transform %s->%s. Error is %s.", - this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), - e.what()); + RCLCPP_ERROR(nodeHandle.get_logger(), + "Cannot get transform %s->%s. Error is %s.", + this->filteringFrame.c_str(), + this->localBoundingBoxFrame.c_str(), e.what()); return; } @@ -1930,7 +1929,7 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( } if (this->computeDebugLocalBoundingBox) { - this->localBoundingBoxDebugMarkerPublisher.publish(boundingBoxDebugMsg); + this->localBoundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); } } @@ -1947,7 +1946,7 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( tf2::toMsg(box.min(), boundingBoxMsg.polygon.points[0]); tf2::toMsg(box.max(), boundingBoxMsg.polygon.points[1]); - this->localBoundingBoxPublisher.publish(boundingBoxMsg); + this->localBoundingBoxPublisher->publish(boundingBoxMsg); if (this->publishLocalBoundingBoxMarker) { visualization_msgs::msg::Marker msg; @@ -1965,7 +1964,7 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( msg.ns = "local_bounding_box"; msg.frame_locked = static_cast(true); - this->localBoundingBoxMarkerPublisher.publish(msg); + this->localBoundingBoxMarkerPublisher->publish(msg); } // compute and publish the scan_point_cloud with robot bounding box removed @@ -1991,13 +1990,14 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2Ptr boxFilteredCloud( + sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud( new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - this->scanPointCloudNoLocalBoundingBoxPublisher.publish(boxFilteredCloud); + this->scanPointCloudNoLocalBoundingBoxPublisher->publish( + *boxFilteredCloud); } } } @@ -2006,7 +2006,7 @@ template void RobotBodyFilter::createBodyVisualizationMsg( const std::map &bodies, - const rclcpp::Time &stamp, const std_msgs::ColorRGBA &color, + const rclcpp::Time &stamp, const std_msgs::msg::ColorRGBA &color, visualization_msgs::msg::MarkerArray &markerArray) const { // when computing the markers for publication, we want to publish them to the // time of the scan, so we need to set cacheLookupBetweenScansRatio again to @@ -2035,75 +2035,79 @@ void RobotBodyFilter::createBodyVisualizationMsg( } } -template -void RobotBodyFilter::robotDescriptionUpdated( - dynamic_reconfigure::ConfigConstPtr newConfig) { - auto robotDescriptionIdx = static_cast(-1); - for (size_t i = 0; i < newConfig->strs.size(); ++i) { - if (newConfig->strs[i].name == this->robotDescriptionUpdatesFieldName) { - robotDescriptionIdx = i; - break; - } - } - - // robot_description parameter was not found, so we don't have to restart the - // filter - if (robotDescriptionIdx == static_cast(-1)) - return; - - auto urdf = newConfig->strs[robotDescriptionIdx].value; - - RCLCPP_INFO(nodeHandle.get_logger(), - "RobotBodyFilter: Reloading robot model because of " - "dynamic_reconfigure update. Filter operation stopped."); - - this->tfFramesWatchdog->pause(); - this->configured_ = false; - - this->clearRobotMask(); - this->addRobotMaskFromUrdf(urdf); - - this->tfFramesWatchdog->unpause(); - this->timeConfigured = rclcpp::Time::now(); - this->configured_ = true; - - RCLCPP_INFO( - nodeHandle.get_logger(), - "RobotBodyFilter: Robot model reloaded, resuming filter operation."); -} - -template -bool RobotBodyFilter::triggerModelReload(std_srvs::TriggerRequest &, - std_srvs::TriggerResponse &) { - std::string urdf; - auto success = this->nodeHandle.getParam(this->robotDescriptionParam, urdf); - - if (!success) { - ROS_ERROR_STREAM("RobotBodyFilter: Parameter " - << this->robotDescriptionParam << " doesn't exist."); - return false; - } - - RCLCPP_INFO( - nodeHandle.get_logger(), - "RobotBodyFilter: Reloading robot model because of trigger. Filter " - "operation stopped."); - - this->tfFramesWatchdog->pause(); - this->configured_ = false; - - this->clearRobotMask(); - this->addRobotMaskFromUrdf(urdf); - - this->tfFramesWatchdog->unpause(); - this->timeConfigured = rclcpp::Time::now(); - this->configured_ = true; - - RCLCPP_INFO( - nodeHandle.get_logger(), - "RobotBodyFilter: Robot model reloaded, resuming filter operation."); - return true; -} +// TODO: update for ROS2 +// template +// void RobotBodyFilter::robotDescriptionUpdated( +// dynamic_reconfigure::ConfigConstPtr newConfig) { +// auto robotDescriptionIdx = static_cast(-1); +// for (size_t i = 0; i < newConfig->strs.size(); ++i) { +// if (newConfig->strs[i].name == this->robotDescriptionUpdatesFieldName) { +// robotDescriptionIdx = i; +// break; +// } +// } + +// // robot_description parameter was not found, so we don't have to restart +// the +// // filter +// if (robotDescriptionIdx == static_cast(-1)) +// return; + +// auto urdf = newConfig->strs[robotDescriptionIdx].value; + +// RCLCPP_INFO(nodeHandle.get_logger(), +// "RobotBodyFilter: Reloading robot model because of " +// "dynamic_reconfigure update. Filter operation stopped."); + +// this->tfFramesWatchdog->pause(); +// this->configured_ = false; + +// this->clearRobotMask(); +// this->addRobotMaskFromUrdf(urdf); + +// this->tfFramesWatchdog->unpause(); +// this->timeConfigured = nodeHandle.now(); +// this->configured_ = true; + +// RCLCPP_INFO( +// nodeHandle.get_logger(), +// "RobotBodyFilter: Robot model reloaded, resuming filter operation."); +// } + +// template +// bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request &, +// std_srvs::srv::Trigger_Response +// &) { +// std::string urdf; +// auto success = this->nodeHandle.getParam(this->robotDescriptionParam, +// urdf); + +// if (!success) { +// ROS_ERROR_STREAM("RobotBodyFilter: Parameter " +// << this->robotDescriptionParam << " doesn't exist."); +// return false; +// } + +// RCLCPP_INFO( +// nodeHandle.get_logger(), +// "RobotBodyFilter: Reloading robot model because of trigger. Filter " +// "operation stopped."); + +// this->tfFramesWatchdog->pause(); +// this->configured_ = false; + +// this->clearRobotMask(); +// this->addRobotMaskFromUrdf(urdf); + +// this->tfFramesWatchdog->unpause(); +// this->timeConfigured = nodeHandle.now(); +// this->configured_ = true; + +// RCLCPP_INFO( +// nodeHandle.get_logger(), +// "RobotBodyFilter: Robot model reloaded, resuming filter operation."); +// return true; +// } template RobotBodyFilter::~RobotBodyFilter() { if (this->tfFramesWatchdog != nullptr) @@ -2112,7 +2116,7 @@ template RobotBodyFilter::~RobotBodyFilter() { template ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest( - const string &linkName) const { + const std::string &linkName) const { return this->getLinkInflationForContainsTest({linkName}); } @@ -2125,7 +2129,7 @@ ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest( template ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest( - const string &linkName) const { + const std::string &linkName) const { return this->getLinkInflationForShadowTest({linkName}); } @@ -2138,7 +2142,7 @@ ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest( template ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere( - const string &linkName) const { + const std::string &linkName) const { return this->getLinkInflationForBoundingSphere({linkName}); } @@ -2151,7 +2155,7 @@ ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere( template ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox( - const string &linkName) const { + const std::string &linkName) const { return this->getLinkInflationForBoundingBox({linkName}); } @@ -2190,7 +2194,9 @@ bool ScaleAndPadding::operator!=(const ScaleAndPadding &other) const { } // namespace robot_body_filter -PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, - filters::FilterBase) -PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, - filters::FilterBase) +#include + +// PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, +// filters::FilterBase) +// PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, +// filters::FilterBase) From e58192c2375412f19846b301ade4f78d9bee1266 Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 16 Apr 2024 19:37:13 -0400 Subject: [PATCH 08/54] started converting params --- src/RobotBodyFilter.cpp | 53 ++++++++++++++++++++++++++++++----------- 1 file changed, 39 insertions(+), 14 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index bb0fd0c..d009574 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -37,7 +37,10 @@ namespace robot_body_filter { template -RobotBodyFilter::RobotBodyFilter() : privateNodeHandle("~") { +RobotBodyFilter::RobotBodyFilter() + : privateNodeHandle("~"), nodeHandle("robot_body_filter"), + modelPoseUpdateInterval(0, 0), reachableTransformTimeout(0, 0), + unreachableTransformTimeout(0, 0), tfBufferLength(0, 0) { this->modelMutex.reset(new std::mutex()); } @@ -55,15 +58,37 @@ template bool RobotBodyFilter::configure() { // data) this->tfBuffer->clear(); } - - // this->fixedFrame = this->getParamVerbose("frames/fixed", "base_link"); - // stripLeadingSlash(this->fixedFrame, true); - // this->sensorFrame = this->getParamVerbose("frames/sensor", ""); - // stripLeadingSlash(this->sensorFrame, true); - // this->filteringFrame = this->getParamVerbose("frames/filtering", - // this->fixedFrame); stripLeadingSlash(this->filteringFrame, true); - // this->minDistance = this->getParamVerbose("sensor/min_distance", 0.0, "m"); - // this->maxDistance = this->getParamVerbose("sensor/max_distance", 0.0, "m"); + { + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + param_desc.description = "frames/fixed"; + this->nodeHandle.declare_parameter("fixedFrame", "base_link", param_desc); + this->nodeHandle.get_parameter("fixedFrame", this->fixedFrame); + stripLeadingSlash(this->fixedFrame, true); + } + { + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + param_desc.description = "frames/sensor"; + this->nodeHandle.declare_parameter("sensorFrame", "", param_desc); + this->nodeHandle.get_parameter("sensorFrame", this->sensorFrame); + stripLeadingSlash(this->sensorFrame, true); + } + { + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + param_desc.description = "frames/filtering"; + this->nodeHandle.declare_parameter("filteringFrame", this->fixedFrame, + param_desc); + this->nodeHandle.get_parameter("filteringFrame", this->filteringFrame); + stripLeadingSlash(this->sensorFrame, true); + } + { + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + param_desc.description = "m"; + param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + this->nodeHandle.declare_parameter("minDistance", 0.0, param_desc); + this->nodeHandle.get_parameter("minDistance", this->minDistance); + this->nodeHandle.declare_parameter("maxDistance", 0.0, param_desc); + this->nodeHandle.get_parameter("maxDistance", this->maxDistance); + } // this->robotDescriptionParam = // this->getParamVerbose("body_model/robot_description_param", // "robot_description"); this->keepCloudsOrganized = @@ -2196,7 +2221,7 @@ bool ScaleAndPadding::operator!=(const ScaleAndPadding &other) const { #include -// PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, -// filters::FilterBase) -// PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, -// filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, + filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, + filters::FilterBase) From 37617c421bbf53eb722b51ec18bc1a30937f0fbe Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Apr 2024 09:32:19 -0400 Subject: [PATCH 09/54] Working on reverting CMake changes --- CMakeLists.txt | 70 ++++++++++++-------------------------------------- 1 file changed, 17 insertions(+), 53 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e38c40..6ff5614 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,59 +1,23 @@ -# ############################################################################### -# Set minimum required version of cmake, project name and compile options -# ############################################################################### -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.7.2) project(robot_body_filter) -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() +set (CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};${CMAKE_CURRENT_SOURCE_DIR}/cmake") +include(DetectOptional) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +if(${ROBOT_BODY_FILTER_HAVE_CXX_OPTIONAL}) + # add_compile_definitions would be nicer, but isn't available in Stretch + add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=1) +else() + add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -# Suppress all warnings -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") - -# ############################################################################### -# Find ament packages and libraries for ament and system dependencies -# ############################################################################### - -# find dependencies -set(THIS_PACKAGE_INCLUDE_DEPENDS - - # dynamic_reconfigure # may be superceeded by using ros2 parameters - filters - geometric_shapes - geometry_msgs - laser_geometry - - # instead of libpcl-all-dev? - pcl_ros - moveit_core - - # moveit_ros_perception - - # roscpp replaced by rclcpp - rclcpp - sensor_msgs - std_srvs - std_msgs - tf2 - tf2_ros - tf2_msgs - urdf - visualization_msgs - pcl_conversions - fcl - tf2_eigen - tf2_sensor_msgs -) +set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core rclcpp sensor_msgs tf2 tf2_ros urdf visualization_msgs) +set(MESSAGE_DEPS geometry_msgs std_msgs) # find dependencies find_package(ament_cmake REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_DEPS}) find_package(${Dependency} REQUIRED) endforeach() @@ -65,7 +29,7 @@ include_directories( ) add_library(${PROJECT_NAME}_filter src/RobotBodyFilter.cpp) -ament_target_dependencies(${PROJECT_NAME}_filter ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(${PROJECT_NAME}_filter ${THIS_PACKAGE_DEPS}) target_include_directories(${PROJECT_NAME}_filter PUBLIC @@ -76,11 +40,11 @@ target_include_directories(${PROJECT_NAME}_filter find_package(rosidl_default_generators REQUIRED) -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/OrientedBoundingBox.msg" - "msg/OrientedBoundingBoxStamped.msg" - DEPENDENCIES builtin_interfaces std_msgs geometry_msgs std_srvs tf2_msgs tf2_sensor_msgs tf2_ros tf2_eigen visualization_msgs - ) +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/OrientedBoundingBox.msg" +# "msg/OrientedBoundingBoxStamped.msg" +# DEPENDENCIES builtin_interfaces std_msgs geometry_msgs std_srvs tf2_msgs tf2_sensor_msgs tf2_ros tf2_eigen visualization_msgs +# ) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() \ No newline at end of file From 9d8dd0bc4152e3f3c3ee6b35990807ab7bbcc549 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Apr 2024 09:57:20 -0400 Subject: [PATCH 10/54] Cleaning up Cmake --- CMakeLists.txt | 20 ++++++++++++++++---- src/RobotBodyFilter.cpp | 1 - 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6ff5614..2faa47f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,16 +11,22 @@ else() add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core rclcpp sensor_msgs tf2 tf2_ros urdf visualization_msgs) +set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs tf2 tf2_ros urdf visualization_msgs) set(MESSAGE_DEPS geometry_msgs std_msgs) # find dependencies find_package(ament_cmake REQUIRED) +find_package(pcl_conversions REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_DEPS}) find_package(${Dependency} REQUIRED) endforeach() +find_package(PCL REQUIRED COMPONENTS common filters) +find_package(Threads REQUIRED) + +find_package(PkgConfig REQUIRED) + # build include_directories( include @@ -28,10 +34,16 @@ include_directories( ${Dependency}_INCLUDE_DIRS ) -add_library(${PROJECT_NAME}_filter src/RobotBodyFilter.cpp) -ament_target_dependencies(${PROJECT_NAME}_filter ${THIS_PACKAGE_DEPS}) +add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} +) +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_DEPS} + ${MESSAGE_DEPS} +) -target_include_directories(${PROJECT_NAME}_filter +target_include_directories(${PROJECT_NAME} PUBLIC $ $ diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index d009574..0c3c226 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1,4 +1,3 @@ -#include #include #include #include From d26d8c616d6f5a4739d3b8f2708e9f0597bd1c5f Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Apr 2024 10:34:16 -0400 Subject: [PATCH 11/54] Started readding tests --- CMakeLists.txt | 97 ++++++++++++++---- .../robot_body_filter/utils/filter_utils.hpp | 2 +- include/robot_body_filter/utils/tf2_eigen.h | 2 +- package.xml | 3 +- test/test_bodies.cpp | 24 ++--- test/test_filter_utils.cpp | 98 ++++++++++--------- 6 files changed, 144 insertions(+), 82 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2faa47f..9f0eece 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,13 +11,11 @@ else() add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs tf2 tf2_ros urdf visualization_msgs) +set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros urdf visualization_msgs GLUT) set(MESSAGE_DEPS geometry_msgs std_msgs) -# find dependencies find_package(ament_cmake REQUIRED) -find_package(pcl_conversions REQUIRED) foreach(Dependency IN ITEMS ${THIS_PACKAGE_DEPS}) find_package(${Dependency} REQUIRED) endforeach() @@ -27,36 +25,95 @@ find_package(Threads REQUIRED) find_package(PkgConfig REQUIRED) -# build +find_package(rosidl_default_generators REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} "msg/OrientedBoundingBox.msg" "msg/OrientedBoundingBoxStamped.msg" DEPENDENCIES ${MESSAGE_DEPS}) + include_directories( include include/utils ${Dependency}_INCLUDE_DIRS ) -add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) -target_link_libraries(${PROJECT_NAME} - ${PCL_LIBRARIES} +set(UTILS_SRCS + src/utils/bodies.cpp +# src/utils/cloud.cpp +# src/utils/shapes.cpp +# src/utils/string_utils.cpp +# src/utils/tf2_eigen.cpp +# src/utils/tf2_sensor_msgs.cpp +# src/utils/time_utils.cpp ) -ament_target_dependencies(${PROJECT_NAME} +add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) +target_link_libraries(${PROJECT_NAME}_utils ${catkin_LIBRARIES} ${LIBFCL_LIBRARIES} ${PCL_LIBRARIES}) + +add_library(lib${PROJECT_NAME} src/RobotBodyFilter.cpp) +target_link_libraries(lib${PROJECT_NAME} + ${PROJECT_NAME}_utils +) +ament_target_dependencies(lib${PROJECT_NAME} ${THIS_PACKAGE_DEPS} - ${MESSAGE_DEPS} ) -target_include_directories(${PROJECT_NAME} - PUBLIC - $ - $ - ${THIS_PACKAGE_INCLUDE_DEPENDS} +target_include_directories(lib${PROJECT_NAME} + PUBLIC + $ + $ + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -find_package(rosidl_default_generators REQUIRED) +if (BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + +# ament_add_gtest(test_set_utils test/test_set_utils.cpp) +# target_link_libraries(test_set_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_shapes_rbf test/test_shapes.cpp) +# target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_urdf_eigen test/test_urdf_eigen.cpp) +# target_link_libraries(test_urdf_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_time_utils test/test_time_utils.cpp) +# target_link_libraries(test_time_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_tf2_eigen test/test_tf2_eigen.cpp) +# target_link_libraries(test_tf2_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) -# rosidl_generate_interfaces(${PROJECT_NAME} -# "msg/OrientedBoundingBox.msg" -# "msg/OrientedBoundingBoxStamped.msg" -# DEPENDENCIES builtin_interfaces std_msgs geometry_msgs std_srvs tf2_msgs tf2_sensor_msgs tf2_ros tf2_eigen visualization_msgs -# ) +# ament_add_gtest(test_string_utils test/test_string_utils.cpp) +# target_link_libraries(test_string_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + + ament_add_gtest(test_bodies test/test_bodies.cpp) + ament_target_dependencies(test_bodies ${THIS_PACKAGE_DEPS}) + target_link_libraries(test_bodies lib${PROJECT_NAME}) + +# ament_add_gtest(test_cloud test/test_cloud.cpp) +# target_link_libraries(test_cloud ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_tf2_sensor_msgs test/test_tf2_sensor_msgs.cpp) +# target_link_libraries(test_tf2_sensor_msgs tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_xmlrpc_traits test/test_xmlrpc_traits.cpp) +# target_link_libraries(test_xmlrpc_traits ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + +# ament_add_gtest(test_filter_utils test/test_filter_utils.test test/test_filter_utils.cpp) +# ament_target_dependencies(test_filter_utils ${THIS_PACKAGE_DEPS}) +# target_link_libraries(test_filter_utils lib${PROJECT_NAME}) + +# ament_add_gtest(test_tf_frames_watchdog test/test_tf_frames_watchdog.cpp) +# target_link_libraries(test_tf_frames_watchdog TFFramesWatchdog ${catkin_LIBRARIES}) + +# ament_add_gtest(test_ray_casting_shape_mask test/test_ray_casting_shape_mask.cpp) +# target_link_libraries(test_ray_casting_shape_mask RayCastingShapeMask ${catkin_LIBRARIES}) +# if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release" OR "${CMAKE_BUILD_TYPE}" STREQUAL "RelWithDebInfo") +# target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=1) +# else() +# target_compile_definitions(test_ray_casting_shape_mask PRIVATE RELEASE_BUILD=0) +# endif() + +# ament_add_gtest(test_robot_body_filter test/test_robot_body_filter.test test/test_robot_body_filter.cpp) +# target_link_libraries(test_robot_body_filter ${PROJECT_NAME} ${catkin_LIBRARIES}) +# target_compile_options(test_robot_body_filter PRIVATE -fno-var-tracking-assignments) # speed up build +endif() ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_package() \ No newline at end of file diff --git a/include/robot_body_filter/utils/filter_utils.hpp b/include/robot_body_filter/utils/filter_utils.hpp index 30dbd13..1b05eaf 100644 --- a/include/robot_body_filter/utils/filter_utils.hpp +++ b/include/robot_body_filter/utils/filter_utils.hpp @@ -5,7 +5,7 @@ // #include // #include -// #include +#include // #include "robot_body_filter/utils/string_utils.hpp" // #include "robot_body_filter/utils/xmlrpc_traits.h" diff --git a/include/robot_body_filter/utils/tf2_eigen.h b/include/robot_body_filter/utils/tf2_eigen.h index 040abeb..2e4434e 100644 --- a/include/robot_body_filter/utils/tf2_eigen.h +++ b/include/robot_body_filter/utils/tf2_eigen.h @@ -2,7 +2,7 @@ #define ROBOT_BODY_FILTER_TF2_EIGEN_H #include -#include +#include namespace tf2 { void toMsg(const Eigen::Vector3d &in, geometry_msgs::msg::Point32 &out); diff --git a/package.xml b/package.xml index 425fc73..65d677b 100644 --- a/package.xml +++ b/package.xml @@ -14,7 +14,7 @@ Martin Pecka - ament_cmake_auto + ament_cmake filters @@ -27,6 +27,7 @@ rclcpp sensor_msgs std_msgs + std_srvs tf2 tf2_ros urdf diff --git a/test/test_bodies.cpp b/test/test_bodies.cpp index 0e1abe4..d2fa40a 100644 --- a/test/test_bodies.cpp +++ b/test/test_bodies.cpp @@ -47,10 +47,10 @@ class WrongBody : public ::bodies::Body { throw std::runtime_error("Should not be called"); } - void computeBoundingBox(OBB& box) const override - { - throw std::runtime_error("Should not be called"); - } + // void computeBoundingBox(OBB& box) const override + // { + // throw std::runtime_error("Should not be called"); + // } BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scaling) const override { throw std::runtime_error("Should not be called"); @@ -109,8 +109,8 @@ TEST(Bodies, ComputeBoundingBoxSphere) sphere->computeBoundingBox(bbox1); body->computeBoundingBox(bbox2); - sphere->computeBoundingBox(obb1); - body->computeBoundingBox(obb2); + // sphere->computeBoundingBox(obb1); + // body->computeBoundingBox(obb2); // check that bboxes are equal regardless we ask through the exact type or Body* EXPECT_EQ(bbox1.min(), bbox2.min()); @@ -153,8 +153,8 @@ TEST(Bodies, ComputeBoundingBoxBox) box->computeBoundingBox(bbox1); body->computeBoundingBox(bbox2); - box->computeBoundingBox(obb1); - body->computeBoundingBox(obb2); + // box->computeBoundingBox(obb1); + // body->computeBoundingBox(obb2); // check that bboxes are equal regardless we ask through the exact type or Body* EXPECT_EQ(bbox1.min(), bbox2.min()); @@ -198,8 +198,8 @@ TEST(Bodies, ComputeBoundingBoxCylinder) cylinder->computeBoundingBox(bbox1); body->computeBoundingBox(bbox2); - cylinder->computeBoundingBox(obb1); - body->computeBoundingBox(obb2); + // cylinder->computeBoundingBox(obb1); + // body->computeBoundingBox(obb2); // check that bboxes are equal regardless we ask through the exact type or Body* EXPECT_EQ(bbox1.min(), bbox2.min()); @@ -246,8 +246,8 @@ TEST(Bodies, ComputeBoundingBoxConvexMesh) mesh->computeBoundingBox(bbox1); body->computeBoundingBox(bbox2); - mesh->computeBoundingBox(obb1); - body->computeBoundingBox(obb2); + // mesh->computeBoundingBox(obb1); + // body->computeBoundingBox(obb2); // check that bboxes are equal regardless we ask through the exact type or Body* EXPECT_EQ(bbox1.min(), bbox2.min()); diff --git a/test/test_filter_utils.cpp b/test/test_filter_utils.cpp index 91bb11d..81d9da0 100644 --- a/test/test_filter_utils.cpp +++ b/test/test_filter_utils.cpp @@ -1,9 +1,13 @@ #include "gtest/gtest.h" -#include +// #include -using namespace robot_body_filter; +#include +#include +#include -class TestFilter : public robot_body_filter::FilterBase +// using namespace robot_body_filter; + +class TestFilter : public filters::FilterBase { public: bool update(const std::string &data_in, std::string &data_out) override { @@ -13,73 +17,73 @@ class TestFilter : public robot_body_filter::FilterBase protected: bool configure() override { bool defaultUsed; - EXPECT_EQ(false, this->getParamVerbose("debug/pcl/inside", true, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ(true, this->getParamVerbose("debug/pcl/nonexistent", true, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ(-1, this->getParamVerbose("test/negative", 1, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ(1, this->getParamVerbose("nonexistent", 1, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ(0.1, this->getParamVerbose("sensor/min_distance", 0.01, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ(0.01, this->getParamVerbose("nonexistent", 0.01, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ("odom", this->getParamVerbose("frames/fixed", std::string("fixed"), "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ("fixed", this->getParamVerbose("nonexistent", std::string("fixed"), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ("odom", this->getParamVerbose("frames/fixed", "fixed", "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ("fixed", this->getParamVerbose("nonexistent", "fixed", "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ("test", this->getParamVerbose("test_value", "test", "", &defaultUsed)); EXPECT_TRUE(defaultUsed); // wrong value type - EXPECT_EQ(1, this->getParamVerbose("frames/fixed", 1, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); // wrong value type - EXPECT_THROW(this->getParamVerbose("test/negative", static_cast(1)), - std::invalid_argument); - EXPECT_EQ(1, this->getParamVerbose("nonexistent", static_cast(1), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_THROW(this->getParamVerbose("test/negative", static_cast(1)), - std::invalid_argument); - EXPECT_EQ(1, this->getParamVerbose("non/existent", static_cast(1), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ(ros::Duration(60), this->getParamVerbose("transforms/buffer_length", - ros::Duration(30), "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ(ros::Duration(30), this->getParamVerbose("nonexistent", ros::Duration(30), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ(std::vector({"antenna", "base_link::big_collision_box"}), - this->getParamVerbose("ignored_links/bounding_sphere", std::vector(), "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ(std::set({"antenna", "base_link::big_collision_box"}), - this->getParamVerboseSet("ignored_links/bounding_sphere", {}, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ(std::set({0, 1}), - this->getParamVerboseSet("nonexistent_set", {0, 1}, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ((std::map({{"antenna::contains", 1.2}, {"antenna::bounding_sphere", 1.2}, {"antenna::bounding_box", 1.2}, {"*::big_collision_box::contains", 2.0}, {"*::big_collision_box::bounding_sphere", 2.0}, {"*::big_collision_box::bounding_box", 2.0}, {"*::big_collision_box::shadow", 3.0}})), - this->getParamVerboseMap("body_model/inflation/per_link/scale", {}, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ((std::map({{"laser::shadow", 0.015}, {"base_link", 0.05}})), - this->getParamVerboseMap("body_model/inflation/per_link/padding", {}, "m", &defaultUsed)); EXPECT_FALSE(defaultUsed); - EXPECT_EQ((std::map({{"a", 1}, {"b", 2}})), - this->getParamVerboseMap("nonexistent_map", {{"a", 1}, {"b", 2}}, "m", &defaultUsed)); EXPECT_TRUE(defaultUsed); - EXPECT_EQ((std::map({{"a", 1}, {"b", 2}})), - this->getParamVerboseMap("body_model/inflation/per_link/all_wrong", {{"a", 1}, {"b", 2}}, "m", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ(false, this->getParamVerbose("debug/pcl/inside", true, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ(true, this->getParamVerbose("debug/pcl/nonexistent", true, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ(-1, this->getParamVerbose("test/negative", 1, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ(1, this->getParamVerbose("nonexistent", 1, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ(0.1, this->getParamVerbose("sensor/min_distance", 0.01, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ(0.01, this->getParamVerbose("nonexistent", 0.01, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ("odom", this->getParamVerbose("frames/fixed", std::string("fixed"), "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ("fixed", this->getParamVerbose("nonexistent", std::string("fixed"), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ("odom", this->getParamVerbose("frames/fixed", "fixed", "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ("fixed", this->getParamVerbose("nonexistent", "fixed", "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ("test", this->getParamVerbose("test_value", "test", "", &defaultUsed)); EXPECT_TRUE(defaultUsed); // wrong value type + // EXPECT_EQ(1, this->getParamVerbose("frames/fixed", 1, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); // wrong value type + // EXPECT_THROW(this->getParamVerbose("test/negative", static_cast(1)), + // std::invalid_argument); + // EXPECT_EQ(1, this->getParamVerbose("nonexistent", static_cast(1), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_THROW(this->getParamVerbose("test/negative", static_cast(1)), + // std::invalid_argument); + // EXPECT_EQ(1, this->getParamVerbose("non/existent", static_cast(1), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ(ros::Duration(60), this->getParamVerbose("transforms/buffer_length", + // ros::Duration(30), "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ(ros::Duration(30), this->getParamVerbose("nonexistent", ros::Duration(30), "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ(std::vector({"antenna", "base_link::big_collision_box"}), + // this->getParamVerbose("ignored_links/bounding_sphere", std::vector(), "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ(std::set({"antenna", "base_link::big_collision_box"}), + // this->getParamVerboseSet("ignored_links/bounding_sphere", {}, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ(std::set({0, 1}), + // this->getParamVerboseSet("nonexistent_set", {0, 1}, "", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ((std::map({{"antenna::contains", 1.2}, {"antenna::bounding_sphere", 1.2}, {"antenna::bounding_box", 1.2}, {"*::big_collision_box::contains", 2.0}, {"*::big_collision_box::bounding_sphere", 2.0}, {"*::big_collision_box::bounding_box", 2.0}, {"*::big_collision_box::shadow", 3.0}})), + // this->getParamVerboseMap("body_model/inflation/per_link/scale", {}, "", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ((std::map({{"laser::shadow", 0.015}, {"base_link", 0.05}})), + // this->getParamVerboseMap("body_model/inflation/per_link/padding", {}, "m", &defaultUsed)); EXPECT_FALSE(defaultUsed); + // EXPECT_EQ((std::map({{"a", 1}, {"b", 2}})), + // this->getParamVerboseMap("nonexistent_map", {{"a", 1}, {"b", 2}}, "m", &defaultUsed)); EXPECT_TRUE(defaultUsed); + // EXPECT_EQ((std::map({{"a", 1}, {"b", 2}})), + // this->getParamVerboseMap("body_model/inflation/per_link/all_wrong", {{"a", 1}, {"b", 2}}, "m", &defaultUsed)); EXPECT_TRUE(defaultUsed); return true; } }; TEST(FilterUtils, getParamVerboseFromDict) { - ros::NodeHandle nh; + rclcpp::Node nh; auto filter = std::make_shared(); auto filterBase = std::dynamic_pointer_cast>(filter); - filterBase->configure("test_dict_config", nh); + // filterBase->configure("test_dict_config", nh); } TEST(FilterUtils, getParamVerboseFromChain) { - ros::NodeHandle nh; + rclcpp::Node nh; auto filter = std::make_shared(); auto filterBase = std::dynamic_pointer_cast>(filter); - XmlRpc::XmlRpcValue value; - nh.getParam("test_chain_config", value); - ASSERT_EQ(XmlRpc::XmlRpcValue::TypeArray, value.getType()); - ASSERT_EQ(1, value.size()); + // XmlRpc::XmlRpcValue value; + // nh.getParam("test_chain_config", value); + // ASSERT_EQ(XmlRpc::XmlRpcValue::TypeArray, value.getType()); + // ASSERT_EQ(1, value.size()); - filterBase->configure(value[0]); + // filterBase->configure(value[0]); } int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_filter_utils"); + rclcpp::init(argc, argv); return RUN_ALL_TESTS(); } \ No newline at end of file From 263fc147b2e316f0256933450d9a6ae89f339153 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Apr 2024 10:35:03 -0400 Subject: [PATCH 12/54] minor things --- CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f0eece..2197cdd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -115,5 +115,5 @@ if (BUILD_TESTING) # target_compile_options(test_robot_body_filter PRIVATE -fno-var-tracking-assignments) # speed up build endif() -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -ament_package() \ No newline at end of file +ament_export_dependencies(${THIS_PACKAGE_DEPS}) +ament_package() From f3daa57f88fd240c2850bb562236cdce9ff6bbe7 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Apr 2024 10:41:58 -0400 Subject: [PATCH 13/54] Tests not working --- CMakeLists.txt | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2197cdd..67ed6f7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,7 +44,10 @@ set(UTILS_SRCS # src/utils/time_utils.cpp ) add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) -target_link_libraries(${PROJECT_NAME}_utils ${catkin_LIBRARIES} ${LIBFCL_LIBRARIES} ${PCL_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME}_utils +${THIS_PACKAGE_DEPS} +) add_library(lib${PROJECT_NAME} src/RobotBodyFilter.cpp) target_link_libraries(lib${PROJECT_NAME} @@ -64,8 +67,9 @@ target_include_directories(lib${PROJECT_NAME} if (BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) -# ament_add_gtest(test_set_utils test/test_set_utils.cpp) -# target_link_libraries(test_set_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + # ament_add_gtest(test_set_utils test/test_set_utils.cpp) + # ament_target_dependencies(test_set_utils ${THIS_PACKAGE_DEPS}) + # target_link_libraries(test_set_utils lib${PROJECT_NAME}) # ament_add_gtest(test_shapes_rbf test/test_shapes.cpp) # target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) @@ -84,7 +88,7 @@ if (BUILD_TESTING) ament_add_gtest(test_bodies test/test_bodies.cpp) ament_target_dependencies(test_bodies ${THIS_PACKAGE_DEPS}) - target_link_libraries(test_bodies lib${PROJECT_NAME}) + target_link_libraries(test_bodies lib${PROJECT_NAME} ${PROJECT_NAME}_utils) # ament_add_gtest(test_cloud test/test_cloud.cpp) # target_link_libraries(test_cloud ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) From ecadaa40af8da2f3033a5cf52cc4b02ec332095a Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 17 Apr 2024 15:21:36 -0400 Subject: [PATCH 14/54] added param declerations --- src/RobotBodyFilter.cpp | 306 ++++++++++++++++++++++++++-------------- 1 file changed, 204 insertions(+), 102 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index d009574..ee034f2 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -42,6 +42,90 @@ RobotBodyFilter::RobotBodyFilter() modelPoseUpdateInterval(0, 0), reachableTransformTimeout(0, 0), unreachableTransformTimeout(0, 0), tfBufferLength(0, 0) { this->modelMutex.reset(new std::mutex()); + + // Declare ROS2 Parameters in node constructor + auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; + + param_desc.description = "frames/fixed"; + this->nodeHandle.declare_parameter("fixedFrame", "base_link", param_desc); + param_desc.description = "frames/sensor"; + this->nodeHandle.declare_parameter("sensorFrame", "", param_desc); + param_desc.description = "frames/filtering"; + this->nodeHandle.declare_parameter("filteringFrame", this->fixedFrame, + param_desc); + param_desc.description = "m"; + param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + this->nodeHandle.declare_parameter("minDistance", 0.0, param_desc); + this->nodeHandle.declare_parameter("maxDistance", 0.0, param_desc); + param_desc.description = "body_model/robot_description_param"; + this->nodeHandle.declare_parameter("robot_description", "", param_desc); + this->nodeHandle.declare_parameter("filter/keep_clouds_organized", true); + param_desc.description = "s"; + this->nodeHandle.declare_parameter("filter/model_pose_update_interval", 0.0, + param_desc); + this->nodeHandle.declare_parameter("filter/do_clipping", true); + this->nodeHandle.declare_parameter("filter/do_contains_test", true); + this->nodeHandle.declare_parameter("filter/do_shadow_test", true); + param_desc.description = "m"; + this->nodeHandle.declare_parameter("filter/max_shadow_distance", + this->maxDistance, param_desc); + param_desc.description = "s"; + this->nodeHandle.declare_parameter("transforms/timeout/reachable", 0.1, + param_desc); + this->nodeHandle.declare_parameter("transforms/timeout/unreachable", 0.2, + param_desc); + this->nodeHandle.declare_parameter("transforms/require_all_reachable", false); + this->nodeHandle.declare_parameter( + "bounding_sphere/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter("bounding_box/publish_cut_out_pointcloud", + false); + this->nodeHandle.declare_parameter( + "oriented_bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter( + "local_bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter("bounding_sphere/compute", false); + this->nodeHandle.declare_parameter("bounding_box/compute", false); + this->nodeHandle.declare_parameter("oriented_bounding_box/compute", false); + this->nodeHandle.declare_parameter("local_bounding_box/compute", false); + this->nodeHandle.declare_parameter("bounding_sphere/debug", false); + this->nodeHandle.declare_parameter("bounding_box/debug", false); + this->nodeHandle.declare_parameter("oriented_bounding_box/debug", false); + this->nodeHandle.declare_parameter("local_bounding_box/debug", false); + this->nodeHandle.declare_parameter("bounding_sphere/marker", false); + this->nodeHandle.declare_parameter("bounding_box/marker", false); + this->nodeHandle.declare_parameter("oriented_bounding_box/marker", false); + this->nodeHandle.declare_parameter("local_bounding_box/marker", false); + this->nodeHandle.declare_parameter("local_bounding_box/frame_id", + this->fixedFrame); + this->nodeHandle.declare_parameter("debug/pcl/inside", false); + this->nodeHandle.declare_parameter("debug/pcl/clip", false); + this->nodeHandle.declare_parameter("debug/pcl/shadow", false); + this->nodeHandle.declare_parameter("debug/marker/contains", false); + this->nodeHandle.declare_parameter("debug/marker/shadow", false); + this->nodeHandle.declare_parameter("debug/marker/bounding_sphere", false); + this->nodeHandle.declare_parameter("debug/marker/bounding_box", false); + param_desc.description = "m"; + this->nodeHandle.declare_parameter("body_model/inflation/padding", 0.0, + param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/scale", 1.0); + // NOTE: Default changed from inflationPadding/inflationScale to 0.0/1.0 + param_desc.description = "m"; + this->nodeHandle.declare_parameter( + "body_model/inflation/contains_test/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/contains_test/scale", + 1.0); + this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/padding", + 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/scale", + 1.0); + this->nodeHandle.declare_parameter( + "body_model/inflation/bounding_sphere/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter( + "body_model/inflation/bounding_sphere/scale", 1.0); + this->nodeHandle.declare_parameter( + "body_model/inflation/bounding_box/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/scale", + 1.0); } template bool RobotBodyFilter::configure() { @@ -58,121 +142,139 @@ template bool RobotBodyFilter::configure() { // data) this->tfBuffer->clear(); } - { - auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; - param_desc.description = "frames/fixed"; - this->nodeHandle.declare_parameter("fixedFrame", "base_link", param_desc); - this->nodeHandle.get_parameter("fixedFrame", this->fixedFrame); - stripLeadingSlash(this->fixedFrame, true); - } - { - auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; - param_desc.description = "frames/sensor"; - this->nodeHandle.declare_parameter("sensorFrame", "", param_desc); - this->nodeHandle.get_parameter("sensorFrame", this->sensorFrame); - stripLeadingSlash(this->sensorFrame, true); - } - { - auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; - param_desc.description = "frames/filtering"; - this->nodeHandle.declare_parameter("filteringFrame", this->fixedFrame, - param_desc); - this->nodeHandle.get_parameter("filteringFrame", this->filteringFrame); - stripLeadingSlash(this->sensorFrame, true); - } - { - auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; - param_desc.description = "m"; - param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - this->nodeHandle.declare_parameter("minDistance", 0.0, param_desc); - this->nodeHandle.get_parameter("minDistance", this->minDistance); - this->nodeHandle.declare_parameter("maxDistance", 0.0, param_desc); - this->nodeHandle.get_parameter("maxDistance", this->maxDistance); - } - // this->robotDescriptionParam = - // this->getParamVerbose("body_model/robot_description_param", - // "robot_description"); this->keepCloudsOrganized = - // this->getParamVerbose("filter/keep_clouds_organized", true); - // this->modelPoseUpdateInterval = - // this->getParamVerbose("filter/model_pose_update_interval", - // rclcpp::Duration(0, 0), "s"); const bool doClipping = - // this->getParamVerbose("filter/do_clipping", true); const bool - // doContainsTest = this->getParamVerbose("filter/do_contains_test", true); - // const bool doShadowTest = this->getParamVerbose("filter/do_shadow_test", - // true); const double maxShadowDistance = - // this->getParamVerbose("filter/max_shadow_distance", this->maxDistance, - // "m"); this->reachableTransformTimeout = - // this->getParamVerbose("transforms/timeout/reachable", - // rclcpp::Duration(0.1), "s"); this->unreachableTransformTimeout = - // this->getParamVerbose("transforms/timeout/unreachable", - // rclcpp::Duration(0.2), "s"); this->requireAllFramesReachable = - // this->getParamVerbose("transforms/require_all_reachable", false); - // this->publishNoBoundingSpherePointcloud = - // this->getParamVerbose("bounding_sphere/publish_cut_out_pointcloud", false); + + this->nodeHandle.get_parameter("fixedFrame", this->fixedFrame); + stripLeadingSlash(this->fixedFrame, true); + + this->nodeHandle.get_parameter("sensorFrame", this->sensorFrame); + stripLeadingSlash(this->sensorFrame, true); + + this->nodeHandle.get_parameter("filteringFrame", this->filteringFrame); + stripLeadingSlash(this->sensorFrame, true); + + this->nodeHandle.get_parameter("minDistance", this->minDistance); + this->nodeHandle.get_parameter("maxDistance", this->maxDistance); + + this->nodeHandle.get_parameter("robot_description", + this->robotDescriptionParam); + + // this->robotDescriptionParam = this->getParamVerbose( + // "body_model/robot_description_param", "robot_description"); + + // this->keepCloudsOrganized = + // this->getParamVerbose("filter/keep_clouds_organized", true); + + // this->modelPoseUpdateInterval = this->getParamVerbose( + // "filter/model_pose_update_interval", rclcpp::Duration(0, 0), "s"); + + // const bool doClipping = this->getParamVerbose("filter/do_clipping", true); + + // const bool doContainsTest = + // this->getParamVerbose("filter/do_contains_test", true); + + // const bool doShadowTest = + // this->getParamVerbose("filter/do_shadow_test", true); + + // const double maxShadowDistance = this->getParamVerbose( + // "filter/max_shadow_distance", this->maxDistance, "m"); + + // this->reachableTransformTimeout = this->getParamVerbose( + // "transforms/timeout/reachable", rclcpp::Duration(0.1), "s"); + + // this->unreachableTransformTimeout = this->getParamVerbose( + // "transforms/timeout/unreachable", rclcpp::Duration(0.2), "s"); + + // this->requireAllFramesReachable = + // this->getParamVerbose("transforms/require_all_reachable", false); + + // this->publishNoBoundingSpherePointcloud = this->getParamVerbose( + // "bounding_sphere/publish_cut_out_pointcloud", false); + // this->publishNoBoundingBoxPointcloud = - // this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", false); - // this->publishNoOrientedBoundingBoxPointcloud = - // this->getParamVerbose("oriented_bounding_box/publish_cut_out_pointcloud", - // false); this->publishNoLocalBoundingBoxPointcloud = - // this->getParamVerbose("local_bounding_box/publish_cut_out_pointcloud", - // false); this->computeBoundingSphere = - // this->getParamVerbose("bounding_sphere/compute", false) || - // this->publishNoBoundingSpherePointcloud; this->computeBoundingBox = - // this->getParamVerbose("bounding_box/compute", false) || - // this->publishNoBoundingBoxPointcloud; this->computeOrientedBoundingBox = - // this->getParamVerbose("oriented_bounding_box/compute", false) || - // this->publishNoOrientedBoundingBoxPointcloud; this->computeLocalBoundingBox - // = this->getParamVerbose("local_bounding_box/compute", false) || - // this->publishNoLocalBoundingBoxPointcloud; this->computeDebugBoundingSphere - // = this->getParamVerbose("bounding_sphere/debug", false); - // this->computeDebugBoundingBox = this->getParamVerbose("bounding_box/debug", - // false); this->computeDebugOrientedBoundingBox = - // this->getParamVerbose("oriented_bounding_box/debug", false); + // this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", + // false); + + // this->publishNoOrientedBoundingBoxPointcloud = this->getParamVerbose( + // "oriented_bounding_box/publish_cut_out_pointcloud", false); + + // this->publishNoLocalBoundingBoxPointcloud = this->getParamVerbose( + // "local_bounding_box/publish_cut_out_pointcloud", false); + + // this->computeBoundingSphere = + // this->getParamVerbose("bounding_sphere/compute", false) || + // this->publishNoBoundingSpherePointcloud; + + // this->computeBoundingBox = + // this->getParamVerbose("bounding_box/compute", false) || + // this->publishNoBoundingBoxPointcloud; + + // this->computeOrientedBoundingBox = + // this->getParamVerbose("oriented_bounding_box/compute", false) || + // this->publishNoOrientedBoundingBoxPointcloud; + + // this->computeLocalBoundingBox = + // this->getParamVerbose("local_bounding_box/compute", false) || + // this->publishNoLocalBoundingBoxPointcloud; + + // this->computeDebugBoundingSphere = + // this->getParamVerbose("bounding_sphere/debug", false); + + // this->computeDebugBoundingBox = + // this->getParamVerbose("bounding_box/debug", false); + + // this->computeDebugOrientedBoundingBox = + // this->getParamVerbose("oriented_bounding_box/debug", false); + // this->computeDebugLocalBoundingBox = - // this->getParamVerbose("local_bounding_box/debug", false); + // this->getParamVerbose("local_bounding_box/debug", false); + // this->publishBoundingSphereMarker = - // this->getParamVerbose("bounding_sphere/marker", false); + // this->getParamVerbose("bounding_sphere/marker", false); + // this->publishBoundingBoxMarker = - // this->getParamVerbose("bounding_box/marker", false); + // this->getParamVerbose("bounding_box/marker", false); + // this->publishOrientedBoundingBoxMarker = - // this->getParamVerbose("oriented_bounding_box/marker", false); + // this->getParamVerbose("oriented_bounding_box/marker", false); + // this->publishLocalBoundingBoxMarker = - // this->getParamVerbose("local_bounding_box/marker", false); + // this->getParamVerbose("local_bounding_box/marker", false); // this->localBoundingBoxFrame = - // this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); - // this->publishDebugPclInside = this->getParamVerbose("debug/pcl/inside", - // false); this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", - // false); this->publishDebugPclShadow = - // this->getParamVerbose("debug/pcl/shadow", false); + // this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); + // this->publishDebugPclInside = + // this->getParamVerbose("debug/pcl/inside", false); + // this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", false); + // this->publishDebugPclShadow = + // this->getParamVerbose("debug/pcl/shadow", false); // this->publishDebugContainsMarker = - // this->getParamVerbose("debug/marker/contains", false); + // this->getParamVerbose("debug/marker/contains", false); // this->publishDebugShadowMarker = - // this->getParamVerbose("debug/marker/shadow", false); + // this->getParamVerbose("debug/marker/shadow", false); // this->publishDebugBsphereMarker = - // this->getParamVerbose("debug/marker/bounding_sphere", false); + // this->getParamVerbose("debug/marker/bounding_sphere", false); // this->publishDebugBboxMarker = - // this->getParamVerbose("debug/marker/bounding_box", false); + // this->getParamVerbose("debug/marker/bounding_box", false); // const auto inflationPadding = - // this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); const auto - // inflationScale = this->getParamVerbose("body_model/inflation/scale", 1.0); - // this->defaultContainsInflation.padding = - // this->getParamVerbose("body_model/inflation/contains_test/padding", - // inflationPadding, "m"); this->defaultContainsInflation.scale = - // this->getParamVerbose("body_model/inflation/contains_test/scale", - // inflationScale); this->defaultShadowInflation.padding = - // this->getParamVerbose("body_model/inflation/shadow_test/padding", - // inflationPadding, "m"); this->defaultShadowInflation.scale = - // this->getParamVerbose("body_model/inflation/shadow_test/scale", - // inflationScale); this->defaultBsphereInflation.padding = - // this->getParamVerbose("body_model/inflation/bounding_sphere/padding", - // inflationPadding, "m"); this->defaultBsphereInflation.scale = - // this->getParamVerbose("body_model/inflation/bounding_sphere/scale", - // inflationScale); this->defaultBboxInflation.padding = - // this->getParamVerbose("body_model/inflation/bounding_box/padding", - // inflationPadding, "m"); this->defaultBboxInflation.scale = - // this->getParamVerbose("body_model/inflation/bounding_box/scale", - // inflationScale); + // this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); + // const auto inflationScale = + // this->getParamVerbose("body_model/inflation/scale", 1.0); + // this->defaultContainsInflation.padding = this->getParamVerbose( + // "body_model/inflation/contains_test/padding", inflationPadding, "m"); + // this->defaultContainsInflation.scale = this->getParamVerbose( + // "body_model/inflation/contains_test/scale", inflationScale); + // this->defaultShadowInflation.padding = this->getParamVerbose( + // "body_model/inflation/shadow_test/padding", inflationPadding, "m"); + // this->defaultShadowInflation.scale = this->getParamVerbose( + // "body_model/inflation/shadow_test/scale", inflationScale); + // this->defaultBsphereInflation.padding = this->getParamVerbose( + // "body_model/inflation/bounding_sphere/padding", inflationPadding, "m"); + // this->defaultBsphereInflation.scale = this->getParamVerbose( + // "body_model/inflation/bounding_sphere/scale", inflationScale); + // this->defaultBboxInflation.padding = this->getParamVerbose( + // "body_model/inflation/bounding_box/padding", inflationPadding, "m"); + // this->defaultBboxInflation.scale = this->getParamVerbose( + // "body_model/inflation/bounding_box/scale", inflationScale); // read per-link padding // const auto perLinkInflationPadding = From f63b0988a41b3350da8f8717aed04eb0728c3512 Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 17 Apr 2024 21:50:44 +0000 Subject: [PATCH 15/54] re-added protected --- include/robot_body_filter/RobotBodyFilter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index e6b6387..a988616 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -106,7 +106,7 @@ template class RobotBodyFilter : public filters::FilterBase { bool update(const T &data_in, T &data_out) override = 0; - // protected: +protected: //! Handle of the node this filter runs in. rclcpp::Node nodeHandle; rclcpp::Node privateNodeHandle; From 985f08f14fa64d821c07ff3bc09e24caff202603 Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 17 Apr 2024 21:50:53 +0000 Subject: [PATCH 16/54] params continued --- src/RobotBodyFilter.cpp | 401 ++++++++++++++++++++++------------------ 1 file changed, 225 insertions(+), 176 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index ee034f2..b5dc52a 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -109,7 +109,6 @@ RobotBodyFilter::RobotBodyFilter() param_desc); this->nodeHandle.declare_parameter("body_model/inflation/scale", 1.0); // NOTE: Default changed from inflationPadding/inflationScale to 0.0/1.0 - param_desc.description = "m"; this->nodeHandle.declare_parameter( "body_model/inflation/contains_test/padding", 0.0, param_desc); this->nodeHandle.declare_parameter("body_model/inflation/contains_test/scale", @@ -126,6 +125,13 @@ RobotBodyFilter::RobotBodyFilter() "body_model/inflation/bounding_box/padding", 0.0, param_desc); this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/scale", 1.0); + + // TODO: This initialization may be incorrect, not sure if I understand how + // this works + // https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html#ae5ab12777100f65bd09163814dbbf486 + // this might need to be initialized with the names of each link + this->nodeHandle.declare_parameters("body_model/inflation/per_link/padding", + std::map()); } template bool RobotBodyFilter::configure() { @@ -145,143 +151,177 @@ template bool RobotBodyFilter::configure() { this->nodeHandle.get_parameter("fixedFrame", this->fixedFrame); stripLeadingSlash(this->fixedFrame, true); - this->nodeHandle.get_parameter("sensorFrame", this->sensorFrame); stripLeadingSlash(this->sensorFrame, true); - this->nodeHandle.get_parameter("filteringFrame", this->filteringFrame); stripLeadingSlash(this->sensorFrame, true); - this->nodeHandle.get_parameter("minDistance", this->minDistance); this->nodeHandle.get_parameter("maxDistance", this->maxDistance); - - this->nodeHandle.get_parameter("robot_description", + this->nodeHandle.get_parameter("body_model/robot_description_param", this->robotDescriptionParam); + this->nodeHandle.get_parameter("filter/keep_clouds_organized", + this->keepCloudsOrganized); + + double tempModelPoseUpdateInterval; + this->nodeHandle.get_parameter("filter/model_pose_update_interval", + tempModelPoseUpdateInterval); + this->modelPoseUpdateInterval = + rclcpp::Duration::from_seconds(tempModelPoseUpdateInterval); + + bool tempDoClipping; + this->nodeHandle.get_parameter("filter/do_clipping", tempDoClipping); + const bool doClipping = tempDoClipping; + + bool tempDoContainsTest; + this->nodeHandle.get_parameter("filter/do_contains_test", tempDoContainsTest); + const bool doContainsTest = tempDoContainsTest; + + bool tempDoShadowTest; + this->nodeHandle.get_parameter("filter/do_shadow_test", tempDoShadowTest); + const bool doShadowTest = tempDoShadowTest; + + double tempMaxShadowDistance; + this->nodeHandle.get_parameter("filter/max_shadow_distance", + tempMaxShadowDistance); + const double maxShadowDistance = tempMaxShadowDistance; + + double tempReachableTransformTimeout; + this->nodeHandle.get_parameter("transforms/timeout/reachable", + tempReachableTransformTimeout); + this->reachableTransformTimeout = + rclcpp::Duration::from_seconds(tempReachableTransformTimeout); + + double tempUnreachableTransformTimeout; + this->nodeHandle.get_parameter("transforms/timeout/unreachable", + tempUnreachableTransformTimeout); + this->unreachableTransformTimeout = + rclcpp::Duration::from_seconds(tempUnreachableTransformTimeout); + + this->nodeHandle.get_parameter("transforms/require_all_reachable", + this->requireAllFramesReachable); + + this->nodeHandle.get_parameter("bounding_sphere/publish_cut_out_pointcloud", + this->publishNoBoundingSpherePointcloud); + + this->nodeHandle.get_parameter("bounding_box/publish_cut_out_pointcloud", + this->publishNoBoundingBoxPointcloud); + + this->nodeHandle.get_parameter( + "oriented_bounding_box/publish_cut_out_pointcloud", + this->publishNoOrientedBoundingBoxPointcloud); + + this->nodeHandle.get_parameter( + "local_bounding_box/publish_cut_out_pointcloud", + this->publishNoLocalBoundingBoxPointcloud); + + this->nodeHandle.get_parameter("bounding_sphere/compute", + this->computeBoundingSphere); + this->computeBoundingSphere = + this->computeBoundingSphere || this->publishNoBoundingSpherePointcloud; + + this->nodeHandle.get_parameter("bounding_box/compute", + this->computeBoundingBox); + this->computeBoundingBox = + this->computeBoundingBox || this->publishNoBoundingBoxPointcloud; + + this->nodeHandle.get_parameter("oriented_bounding_box/compute", + this->computeOrientedBoundingBox); + this->computeOrientedBoundingBox = + this->computeOrientedBoundingBox || + this->publishNoOrientedBoundingBoxPointcloud; + + this->nodeHandle.get_parameter("local_bounding_box/compute", + this->computeLocalBoundingBox); + this->computeLocalBoundingBox = this->computeLocalBoundingBox || + this->publishNoLocalBoundingBoxPointcloud; + + this->nodeHandle.get_parameter("bounding_sphere/debug", + this->computeDebugBoundingSphere); + + this->nodeHandle.get_parameter("bounding_box/debug", + this->computeDebugBoundingBox); + + this->nodeHandle.get_parameter("oriented_bounding_box/debug", + this->computeDebugOrientedBoundingBox); + + this->nodeHandle.get_parameter("local_bounding_box/debug", + this->computeDebugLocalBoundingBox); + + this->nodeHandle.get_parameter("bounding_sphere/marker", + this->publishBoundingSphereMarker); + + this->nodeHandle.get_parameter("bounding_box/marker", + this->publishBoundingBoxMarker); + + this->nodeHandle.get_parameter("oriented_bounding_box/marker", + this->publishOrientedBoundingBoxMarker); - // this->robotDescriptionParam = this->getParamVerbose( - // "body_model/robot_description_param", "robot_description"); + this->nodeHandle.get_parameter("local_bounding_box/marker", + this->publishLocalBoundingBoxMarker); - // this->keepCloudsOrganized = - // this->getParamVerbose("filter/keep_clouds_organized", true); + this->nodeHandle.get_parameter("local_bounding_box/frame_id", + this->localBoundingBoxFrame); - // this->modelPoseUpdateInterval = this->getParamVerbose( - // "filter/model_pose_update_interval", rclcpp::Duration(0, 0), "s"); + this->nodeHandle.get_parameter("debug/pcl/inside", + this->publishDebugPclInside); - // const bool doClipping = this->getParamVerbose("filter/do_clipping", true); - - // const bool doContainsTest = - // this->getParamVerbose("filter/do_contains_test", true); - - // const bool doShadowTest = - // this->getParamVerbose("filter/do_shadow_test", true); - - // const double maxShadowDistance = this->getParamVerbose( - // "filter/max_shadow_distance", this->maxDistance, "m"); - - // this->reachableTransformTimeout = this->getParamVerbose( - // "transforms/timeout/reachable", rclcpp::Duration(0.1), "s"); - - // this->unreachableTransformTimeout = this->getParamVerbose( - // "transforms/timeout/unreachable", rclcpp::Duration(0.2), "s"); - - // this->requireAllFramesReachable = - // this->getParamVerbose("transforms/require_all_reachable", false); - - // this->publishNoBoundingSpherePointcloud = this->getParamVerbose( - // "bounding_sphere/publish_cut_out_pointcloud", false); - - // this->publishNoBoundingBoxPointcloud = - // this->getParamVerbose("bounding_box/publish_cut_out_pointcloud", - // false); - - // this->publishNoOrientedBoundingBoxPointcloud = this->getParamVerbose( - // "oriented_bounding_box/publish_cut_out_pointcloud", false); - - // this->publishNoLocalBoundingBoxPointcloud = this->getParamVerbose( - // "local_bounding_box/publish_cut_out_pointcloud", false); - - // this->computeBoundingSphere = - // this->getParamVerbose("bounding_sphere/compute", false) || - // this->publishNoBoundingSpherePointcloud; - - // this->computeBoundingBox = - // this->getParamVerbose("bounding_box/compute", false) || - // this->publishNoBoundingBoxPointcloud; - - // this->computeOrientedBoundingBox = - // this->getParamVerbose("oriented_bounding_box/compute", false) || - // this->publishNoOrientedBoundingBoxPointcloud; - - // this->computeLocalBoundingBox = - // this->getParamVerbose("local_bounding_box/compute", false) || - // this->publishNoLocalBoundingBoxPointcloud; - - // this->computeDebugBoundingSphere = - // this->getParamVerbose("bounding_sphere/debug", false); - - // this->computeDebugBoundingBox = - // this->getParamVerbose("bounding_box/debug", false); - - // this->computeDebugOrientedBoundingBox = - // this->getParamVerbose("oriented_bounding_box/debug", false); - - // this->computeDebugLocalBoundingBox = - // this->getParamVerbose("local_bounding_box/debug", false); - - // this->publishBoundingSphereMarker = - // this->getParamVerbose("bounding_sphere/marker", false); - - // this->publishBoundingBoxMarker = - // this->getParamVerbose("bounding_box/marker", false); - - // this->publishOrientedBoundingBoxMarker = - // this->getParamVerbose("oriented_bounding_box/marker", false); - - // this->publishLocalBoundingBoxMarker = - // this->getParamVerbose("local_bounding_box/marker", false); - // this->localBoundingBoxFrame = - // this->getParamVerbose("local_bounding_box/frame_id", this->fixedFrame); - // this->publishDebugPclInside = - // this->getParamVerbose("debug/pcl/inside", false); - // this->publishDebugPclClip = this->getParamVerbose("debug/pcl/clip", false); - // this->publishDebugPclShadow = - // this->getParamVerbose("debug/pcl/shadow", false); - // this->publishDebugContainsMarker = - // this->getParamVerbose("debug/marker/contains", false); - // this->publishDebugShadowMarker = - // this->getParamVerbose("debug/marker/shadow", false); - // this->publishDebugBsphereMarker = - // this->getParamVerbose("debug/marker/bounding_sphere", false); - // this->publishDebugBboxMarker = - // this->getParamVerbose("debug/marker/bounding_box", false); - - // const auto inflationPadding = - // this->getParamVerbose("body_model/inflation/padding", 0.0, "m"); - // const auto inflationScale = - // this->getParamVerbose("body_model/inflation/scale", 1.0); - // this->defaultContainsInflation.padding = this->getParamVerbose( - // "body_model/inflation/contains_test/padding", inflationPadding, "m"); - // this->defaultContainsInflation.scale = this->getParamVerbose( - // "body_model/inflation/contains_test/scale", inflationScale); - // this->defaultShadowInflation.padding = this->getParamVerbose( - // "body_model/inflation/shadow_test/padding", inflationPadding, "m"); - // this->defaultShadowInflation.scale = this->getParamVerbose( - // "body_model/inflation/shadow_test/scale", inflationScale); - // this->defaultBsphereInflation.padding = this->getParamVerbose( - // "body_model/inflation/bounding_sphere/padding", inflationPadding, "m"); - // this->defaultBsphereInflation.scale = this->getParamVerbose( - // "body_model/inflation/bounding_sphere/scale", inflationScale); - // this->defaultBboxInflation.padding = this->getParamVerbose( - // "body_model/inflation/bounding_box/padding", inflationPadding, "m"); - // this->defaultBboxInflation.scale = this->getParamVerbose( - // "body_model/inflation/bounding_box/scale", inflationScale); + this->nodeHandle.get_parameter("debug/pcl/clip", this->publishDebugPclClip); + + this->nodeHandle.get_parameter("debug/pcl/shadow", + this->publishDebugPclShadow); + + this->nodeHandle.get_parameter("debug/marker/contains", + this->publishDebugContainsMarker); + + this->nodeHandle.get_parameter("debug/marker/shadow", + this->publishDebugShadowMarker); + + this->nodeHandle.get_parameter("debug/marker/bounding_sphere", + this->publishDebugBsphereMarker); + + this->nodeHandle.get_parameter("debug/marker/bounding_box", + this->publishDebugBboxMarker); + + double tempInflationPadding; + this->nodeHandle.get_parameter("body_model/inflation/padding", + tempInflationPadding); + const double inflationPadding = tempInflationPadding; + + double tempInflationScale; + this->nodeHandle.get_parameter("body_model/inflation/scale", + tempInflationScale); + const double inflationScale = tempInflationScale; + + this->nodeHandle.get_parameter("body_model/inflation/contains_test/padding", + this->defaultContainsInflation.padding); + + this->nodeHandle.get_parameter("body_model/inflation/contains_test/scale", + this->defaultContainsInflation.scale); + + this->nodeHandle.get_parameter("body_model/inflation/shadow_test/padding", + this->defaultShadowInflation.padding); + + this->nodeHandle.get_parameter("body_model/inflation/shadow_test/scale", + this->defaultShadowInflation.scale); + + this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/padding", + this->defaultBsphereInflation.padding); + + this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/scale", + this->defaultBsphereInflation.scale); + + this->nodeHandle.get_parameter("body_model/inflation/bounding_box/padding", + this->defaultBboxInflation.padding); + + this->nodeHandle.get_parameter("body_model/inflation/bounding_box/scale", + this->defaultBboxInflation.scale); // read per-link padding - // const auto perLinkInflationPadding = - // this->getParamVerboseMap("body_model/inflation/per_link/padding", - // std::map(), "m"); for (const auto& inflationPair : - // perLinkInflationPadding) - // { + std::map perLinkInflationPadding; + this->nodeHandle.get_parameters("body_model/inflation/per_link/padding", + perLinkInflationPadding); + + // for (const auto &inflationPair : perLinkInflationPadding) { // bool containsOnly; // bool shadowOnly; // bool bsphereOnly; @@ -294,21 +334,17 @@ template bool RobotBodyFilter::configure() { // linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); // if (!shadowOnly && !bsphereOnly && !bboxOnly) - // this->perLinkContainsInflation[linkName] = - // ScaleAndPadding(this->defaultContainsInflation.scale, - // inflationPair.second); + // this->perLinkContainsInflation[linkName] = ScaleAndPadding( + // this->defaultContainsInflation.scale, inflationPair.second); // if (!containsOnly && !bsphereOnly && !bboxOnly) - // this->perLinkShadowInflation[linkName] = - // ScaleAndPadding(this->defaultShadowInflation.scale, - // inflationPair.second); + // this->perLinkShadowInflation[linkName] = ScaleAndPadding( + // this->defaultShadowInflation.scale, inflationPair.second); // if (!containsOnly && !shadowOnly && !bboxOnly) - // this->perLinkBsphereInflation[linkName] = - // ScaleAndPadding(this->defaultBsphereInflation.scale, - // inflationPair.second); + // this->perLinkBsphereInflation[linkName] = ScaleAndPadding( + // this->defaultBsphereInflation.scale, inflationPair.second); // if (!containsOnly && !shadowOnly && !bsphereOnly) - // this->perLinkBboxInflation[linkName] = - // ScaleAndPadding(this->defaultBboxInflation.scale, - // inflationPair.second); + // this->perLinkBboxInflation[linkName] = ScaleAndPadding( + // this->defaultBboxInflation.scale, inflationPair.second); // } // read per-link scale @@ -348,7 +384,8 @@ template bool RobotBodyFilter::configure() { // ScaleAndPadding(inflationPair.second, // this->defaultShadowInflation.padding); // else - // this->perLinkShadowInflation[linkName].scale = inflationPair.second; + // this->perLinkShadowInflation[linkName].scale = + // inflationPair.second; // } // if (!containsOnly && !shadowOnly && !bboxOnly) @@ -359,7 +396,8 @@ template bool RobotBodyFilter::configure() { // ScaleAndPadding(inflationPair.second, // this->defaultBsphereInflation.padding); // else - // this->perLinkBsphereInflation[linkName].scale = inflationPair.second; + // this->perLinkBsphereInflation[linkName].scale = + // inflationPair.second; // } // if (!containsOnly && !shadowOnly && !bsphereOnly) @@ -370,12 +408,13 @@ template bool RobotBodyFilter::configure() { // ScaleAndPadding(inflationPair.second, // this->defaultBboxInflation.padding); // else - // this->perLinkBboxInflation[linkName].scale = inflationPair.second; + // this->perLinkBboxInflation[linkName].scale = + // inflationPair.second; // } // } - // can contain either whole link names, or scoped names of their collisions - // (i.e. "link::collision_1" or "link::my_collision") + // can contain either whole link names, or scoped names of their + // collisions (i.e. "link::collision_1" or "link::my_collision") // this->linksIgnoredInBoundingSphere = this->template // getParamVerboseSet("ignored_links/bounding_sphere"); // this->linksIgnoredInBoundingBox = this->template @@ -385,8 +424,8 @@ template bool RobotBodyFilter::configure() { // this->linksIgnoredInShadowTest = this->template // getParamVerboseSet("ignored_links/shadow_test", { "laser" }); // this->linksIgnoredEverywhere = this->template - // getParamVerboseSet("ignored_links/everywhere"); this->onlyLinks = - // this->template getParamVerboseSet("only_links"); + // getParamVerboseSet("ignored_links/everywhere"); this->onlyLinks + // = this->template getParamVerboseSet("only_links"); // this->robotDescriptionUpdatesFieldName = // this->getParamVerbose("body_model/dynamic_robot_description/field_name", @@ -408,7 +447,8 @@ template bool RobotBodyFilter::configure() { // if (this->computeBoundingBox) { // this->boundingBoxPublisher = this->nodeHandle.template - // advertise("robot_bounding_box", 100); + // advertise("robot_bounding_box", + // 100); // } // if (this->computeOrientedBoundingBox) { @@ -442,7 +482,8 @@ template bool RobotBodyFilter::configure() { // 100); // } - // if (this->publishLocalBoundingBoxMarker && this->computeLocalBoundingBox) { + // if (this->publishLocalBoundingBoxMarker && + // this->computeLocalBoundingBox) { // this->localBoundingBoxMarkerPublisher = this->nodeHandle.template // advertise("robot_local_bounding_box_marker", // 100); @@ -450,7 +491,8 @@ template bool RobotBodyFilter::configure() { // if (this->publishNoBoundingBoxPointcloud) // { - // this->scanPointCloudNoBoundingBoxPublisher = this->nodeHandle.template + // this->scanPointCloudNoBoundingBoxPublisher = + // this->nodeHandle.template // advertise("scan_point_cloud_no_bbox", // 100); // } @@ -473,7 +515,8 @@ template bool RobotBodyFilter::configure() { // if (this->publishNoBoundingSpherePointcloud) // { - // this->scanPointCloudNoBoundingSpherePublisher = this->nodeHandle.template + // this->scanPointCloudNoBoundingSpherePublisher = + // this->nodeHandle.template // advertise("scan_point_cloud_no_bsphere", // 100); // } @@ -481,19 +524,22 @@ template bool RobotBodyFilter::configure() { // if (this->publishDebugPclInside) // { // this->debugPointCloudInsidePublisher = this->nodeHandle.template - // advertise("scan_point_cloud_inside", 100); + // advertise("scan_point_cloud_inside", + // 100); // } // if (this->publishDebugPclClip) // { // this->debugPointCloudClipPublisher = this->nodeHandle.template - // advertise("scan_point_cloud_clip", 100); + // advertise("scan_point_cloud_clip", + // 100); // } // if (this->publishDebugPclShadow) // { // this->debugPointCloudShadowPublisher = this->nodeHandle.template - // advertise("scan_point_cloud_shadow", 100); + // advertise("scan_point_cloud_shadow", + // 100); // } // if (this->publishDebugContainsMarker) @@ -531,13 +577,15 @@ template bool RobotBodyFilter::configure() { // } // if (this->computeDebugOrientedBoundingBox) { - // this->orientedBoundingBoxDebugMarkerPublisher = this->nodeHandle.template + // this->orientedBoundingBoxDebugMarkerPublisher = + // this->nodeHandle.template // advertise( // "robot_oriented_bounding_box_debug", 100); // } // if (this->computeDebugLocalBoundingBox) { - // this->localBoundingBoxDebugMarkerPublisher = this->nodeHandle.template + // this->localBoundingBoxDebugMarkerPublisher = + // this->nodeHandle.template // advertise( // "robot_local_bounding_box_debug", 100); // } @@ -550,14 +598,14 @@ template bool RobotBodyFilter::configure() { // initialize the 3D body masking tool // auto getShapeTransformCallback = - // std::bind(&RobotBodyFilter::getShapeTransform, this, std::placeholders::_1, - // std::placeholders::_2); shapeMask = + // std::bind(&RobotBodyFilter::getShapeTransform, this, + // std::placeholders::_1, std::placeholders::_2); shapeMask = // std::make_unique(getShapeTransformCallback, // this->minDistance, this->maxDistance, // doClipping, doContainsTest, doShadowTest, maxShadowDistance); - // // the other case happens when configure() is called again from update() - // (e.g. when a new bag file + // // the other case happens when configure() is called again from + // update() (e.g. when a new bag file // // started playing) // if (this->tfFramesWatchdog == nullptr) { // std::set initialMonitoredFrames; @@ -577,25 +625,26 @@ template bool RobotBodyFilter::configure() { // { // initialize the robot body to be masked out // std::string robotUrdf; - // while (!this->nodeHandle.getParam(this->robotDescriptionParam, robotUrdf) + // while (!this->nodeHandle.getParam(this->robotDescriptionParam, + // robotUrdf) // || robotUrdf.length() == 0) { // if (this->failWithoutRobotDescription) // { // throw std::runtime_error( - // "RobotBodyFilter: " + this->robotDescriptionParam + " is empty or - // not set."); + // "RobotBodyFilter: " + this->robotDescriptionParam + " is + // empty or not set."); // } // if (!rclcpp::ok()) // return false; - // ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide the - // robot model. Waiting 1s.", + // ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide + // the robot model. Waiting 1s.", // robotDescriptionParam.c_str()); // rclcpp::Duration(1.0).sleep(); // } - // // happens when configure() is called again from update() (e.g. when a - // new bag file started + // // happens when configure() is called again from update() (e.g. when + // a new bag file started // // playing) // if (!this->shapesToLinks.empty()) // this->clearRobotMask(); @@ -603,10 +652,10 @@ template bool RobotBodyFilter::configure() { // } // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Successfully - // configured."); RCLCPP_INFO(nodeHandle.get_logger(),"Filtering data in frame - // %s", this->filteringFrame.c_str()); - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering into the - // following categories:"); + // configured."); RCLCPP_INFO(nodeHandle.get_logger(),"Filtering data in + // frame %s", this->filteringFrame.c_str()); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering into + // the following categories:"); // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: \tOUTSIDE"); // if (doClipping) RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: // \tCLIP"); if (doContainsTest) @@ -616,20 +665,20 @@ template bool RobotBodyFilter::configure() { // if (this->onlyLinks.empty()) { // if (this->linksIgnoredEverywhere.empty()) { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied - // to all links."); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering + // applied to all links."); // } else { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied - // to all links except %s.", + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering + // applied to all links except %s.", // to_string(this->linksIgnoredEverywhere).c_str()); // } // } else { // if (this->linksIgnoredEverywhere.empty()) { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied - // to links %s.", to_string(this->onlyLinks).c_str()); + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering + // applied to links %s.", to_string(this->onlyLinks).c_str()); // } else { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied - // to links %s with these links excluded: %s.", + // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering + // applied to links %s with these links excluded: %s.", // to_string(this->onlyLinks).c_str(), // to_string(this->linksIgnoredEverywhere).c_str()); // } From a18523ba23f1b0f4a5dd6ff20165b75dcbe956e8 Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 17 Apr 2024 22:55:56 +0000 Subject: [PATCH 17/54] finished get params --- src/RobotBodyFilter.cpp | 232 +++++++++++++++++++++++----------------- 1 file changed, 131 insertions(+), 101 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index b5dc52a..7833aa1 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -132,6 +132,20 @@ RobotBodyFilter::RobotBodyFilter() // this might need to be initialized with the names of each link this->nodeHandle.declare_parameters("body_model/inflation/per_link/padding", std::map()); + + this->nodeHandle.declare_parameters("body_model/inflation/per_link/scale", + std::map()); + + // Note: some of these are init to empty strings, originally they are vectors + // of strings + this->nodeHandle.declare_parameter("ignored_links/bounding_sphere", ""); + this->nodeHandle.declare_parameter("ignored_links/bounding_box", ""); + this->nodeHandle.declare_parameter("ignored_links/contains_test", ""); + this->nodeHandle.declare_parameter("ignored_links/shadow_test", "laser"); + this->nodeHandle.declare_parameter("ignored_links/everywhere", ""); + this->nodeHandle.declare_parameter("only_links", ""); + this->nodeHandle.declare_parameter( + "body_model/dynamic_robot_description/field_name", "robot_model"); } template bool RobotBodyFilter::configure() { @@ -321,115 +335,131 @@ template bool RobotBodyFilter::configure() { this->nodeHandle.get_parameters("body_model/inflation/per_link/padding", perLinkInflationPadding); - // for (const auto &inflationPair : perLinkInflationPadding) { - // bool containsOnly; - // bool shadowOnly; - // bool bsphereOnly; - // bool bboxOnly; - - // auto linkName = inflationPair.first; - // linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); - // linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); - // linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); - // linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); - - // if (!shadowOnly && !bsphereOnly && !bboxOnly) - // this->perLinkContainsInflation[linkName] = ScaleAndPadding( - // this->defaultContainsInflation.scale, inflationPair.second); - // if (!containsOnly && !bsphereOnly && !bboxOnly) - // this->perLinkShadowInflation[linkName] = ScaleAndPadding( - // this->defaultShadowInflation.scale, inflationPair.second); - // if (!containsOnly && !shadowOnly && !bboxOnly) - // this->perLinkBsphereInflation[linkName] = ScaleAndPadding( - // this->defaultBsphereInflation.scale, inflationPair.second); - // if (!containsOnly && !shadowOnly && !bsphereOnly) - // this->perLinkBboxInflation[linkName] = ScaleAndPadding( - // this->defaultBboxInflation.scale, inflationPair.second); - // } + for (const auto &inflationPair : perLinkInflationPadding) { + bool containsOnly; + bool shadowOnly; + bool bsphereOnly; + bool bboxOnly; + + auto linkName = inflationPair.first; + linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); + linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); + linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); + linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); + + if (!shadowOnly && !bsphereOnly && !bboxOnly) + this->perLinkContainsInflation[linkName] = ScaleAndPadding( + this->defaultContainsInflation.scale, inflationPair.second); + if (!containsOnly && !bsphereOnly && !bboxOnly) + this->perLinkShadowInflation[linkName] = ScaleAndPadding( + this->defaultShadowInflation.scale, inflationPair.second); + if (!containsOnly && !shadowOnly && !bboxOnly) + this->perLinkBsphereInflation[linkName] = ScaleAndPadding( + this->defaultBsphereInflation.scale, inflationPair.second); + if (!containsOnly && !shadowOnly && !bsphereOnly) + this->perLinkBboxInflation[linkName] = ScaleAndPadding( + this->defaultBboxInflation.scale, inflationPair.second); + } // read per-link scale - // const auto perLinkInflationScale = - // this->getParamVerboseMap("body_model/inflation/per_link/scale", - // std::map()); for (const auto& inflationPair : - // perLinkInflationScale) - // { - // bool containsOnly; - // bool shadowOnly; - // bool bsphereOnly; - // bool bboxOnly; - - // auto linkName = inflationPair.first; - // linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); - // linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); - // linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); - // linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); - - // if (!shadowOnly && !bsphereOnly && !bboxOnly) - // { - // if (this->perLinkContainsInflation.find(linkName) == - // this->perLinkContainsInflation.end()) - // this->perLinkContainsInflation[linkName] = - // ScaleAndPadding(inflationPair.second, - // this->defaultContainsInflation.padding); - // else - // this->perLinkContainsInflation[linkName].scale = - // inflationPair.second; - // } + std::map perLinkInflationScale; + this->nodeHandle.get_parameters("body_model/inflation/per_link/scale", + perLinkInflationScale); + + for (const auto &inflationPair : perLinkInflationScale) { + bool containsOnly; + bool shadowOnly; + bool bsphereOnly; + bool bboxOnly; + + auto linkName = inflationPair.first; + linkName = removeSuffix(linkName, CONTAINS_SUFFIX, &containsOnly); + linkName = removeSuffix(linkName, SHADOW_SUFFIX, &shadowOnly); + linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); + linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); + + if (!shadowOnly && !bsphereOnly && !bboxOnly) { + if (this->perLinkContainsInflation.find(linkName) == + this->perLinkContainsInflation.end()) + this->perLinkContainsInflation[linkName] = ScaleAndPadding( + inflationPair.second, this->defaultContainsInflation.padding); + else + this->perLinkContainsInflation[linkName].scale = inflationPair.second; + } - // if (!containsOnly && !bsphereOnly && !bboxOnly) - // { - // if (this->perLinkShadowInflation.find(linkName) == - // this->perLinkShadowInflation.end()) - // this->perLinkShadowInflation[linkName] = - // ScaleAndPadding(inflationPair.second, - // this->defaultShadowInflation.padding); - // else - // this->perLinkShadowInflation[linkName].scale = - // inflationPair.second; - // } + if (!containsOnly && !bsphereOnly && !bboxOnly) { + if (this->perLinkShadowInflation.find(linkName) == + this->perLinkShadowInflation.end()) + this->perLinkShadowInflation[linkName] = ScaleAndPadding( + inflationPair.second, this->defaultShadowInflation.padding); + else + this->perLinkShadowInflation[linkName].scale = inflationPair.second; + } - // if (!containsOnly && !shadowOnly && !bboxOnly) - // { - // if (this->perLinkBsphereInflation.find(linkName) == - // this->perLinkBsphereInflation.end()) - // this->perLinkBsphereInflation[linkName] = - // ScaleAndPadding(inflationPair.second, - // this->defaultBsphereInflation.padding); - // else - // this->perLinkBsphereInflation[linkName].scale = - // inflationPair.second; - // } + if (!containsOnly && !shadowOnly && !bboxOnly) { + if (this->perLinkBsphereInflation.find(linkName) == + this->perLinkBsphereInflation.end()) + this->perLinkBsphereInflation[linkName] = ScaleAndPadding( + inflationPair.second, this->defaultBsphereInflation.padding); + else + this->perLinkBsphereInflation[linkName].scale = inflationPair.second; + } - // if (!containsOnly && !shadowOnly && !bsphereOnly) - // { - // if (this->perLinkBboxInflation.find(linkName) == - // this->perLinkBboxInflation.end()) - // this->perLinkBboxInflation[linkName] = - // ScaleAndPadding(inflationPair.second, - // this->defaultBboxInflation.padding); - // else - // this->perLinkBboxInflation[linkName].scale = - // inflationPair.second; - // } - // } + if (!containsOnly && !shadowOnly && !bsphereOnly) { + if (this->perLinkBboxInflation.find(linkName) == + this->perLinkBboxInflation.end()) + this->perLinkBboxInflation[linkName] = ScaleAndPadding( + inflationPair.second, this->defaultBboxInflation.padding); + else + this->perLinkBboxInflation[linkName].scale = inflationPair.second; + } + } // can contain either whole link names, or scoped names of their // collisions (i.e. "link::collision_1" or "link::my_collision") - // this->linksIgnoredInBoundingSphere = this->template - // getParamVerboseSet("ignored_links/bounding_sphere"); - // this->linksIgnoredInBoundingBox = this->template - // getParamVerboseSet("ignored_links/bounding_box"); - // this->linksIgnoredInContainsTest = this->template - // getParamVerboseSet("ignored_links/contains_test"); - // this->linksIgnoredInShadowTest = this->template - // getParamVerboseSet("ignored_links/shadow_test", { "laser" }); - // this->linksIgnoredEverywhere = this->template - // getParamVerboseSet("ignored_links/everywhere"); this->onlyLinks - // = this->template getParamVerboseSet("only_links"); - - // this->robotDescriptionUpdatesFieldName = - // this->getParamVerbose("body_model/dynamic_robot_description/field_name", - // "robot_model"); + // Note: ROS2 does not by default have a set parameter, this was the + // workaround + std::vector tempLinksIgnoredInBoundingSphereVector; + this->nodeHandle.get_parameter("ignored_links/bounding_sphere", + tempLinksIgnoredInBoundingSphereVector); + this->linksIgnoredInBoundingSphere.insert( + tempLinksIgnoredInBoundingSphereVector.begin(), + tempLinksIgnoredInBoundingSphereVector.end()); + + std::vector tempLinksIgnoredInBoundingBoxVector; + this->nodeHandle.get_parameter("ignored_links/bounding_box", + tempLinksIgnoredInBoundingBoxVector); + this->linksIgnoredInBoundingBox.insert( + tempLinksIgnoredInBoundingBoxVector.begin(), + tempLinksIgnoredInBoundingBoxVector.end()); + + std::vector tempLinksIgnoredInContainsTest; + this->nodeHandle.get_parameter("ignored_links/contains_test", + tempLinksIgnoredInContainsTest); + this->linksIgnoredInContainsTest.insert( + tempLinksIgnoredInContainsTest.begin(), + tempLinksIgnoredInContainsTest.end()); + + std::vector tempLinksIgnoredInShadowTest; + this->nodeHandle.get_parameter("ignored_links/shadow_test", + tempLinksIgnoredInShadowTest); + this->linksIgnoredInShadowTest.insert(tempLinksIgnoredInShadowTest.begin(), + tempLinksIgnoredInShadowTest.end()); + + std::vector tempLinksIgnoredEverywhere; + this->nodeHandle.get_parameter("ignored_links/everywhere", + tempLinksIgnoredEverywhere); + this->linksIgnoredEverywhere.insert(tempLinksIgnoredEverywhere.begin(), + tempLinksIgnoredEverywhere.end()); + + std::vector tempOnlyLinks; + this->nodeHandle.get_parameter("only_links", tempOnlyLinks); + this->onlyLinks.insert(tempOnlyLinks.begin(), tempOnlyLinks.end()); + + this->nodeHandle.get_parameter( + "body_model/dynamic_robot_description/field_name", + this->robotDescriptionUpdatesFieldName); + // // subscribe for robot_description param changes // this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( // "dynamic_robot_model_server/parameter_updates", 10, From 326b2370d44d3bb0efb3be136711d8b5df95a6bc Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 17 Apr 2024 23:49:11 +0000 Subject: [PATCH 18/54] added sphere msg, finished params --- CMakeLists.txt | 2 + src/RobotBodyFilter.cpp | 96 +++++++++++++++++++++-------------------- 2 files changed, 52 insertions(+), 46 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e38c40..bf7b105 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -79,6 +79,8 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/OrientedBoundingBox.msg" "msg/OrientedBoundingBoxStamped.msg" + "msg/Sphere.msg" + "msg/SphereStamped.msg" DEPENDENCIES builtin_interfaces std_msgs geometry_msgs std_srvs tf2_msgs tf2_sensor_msgs tf2_ros tf2_eigen visualization_msgs ) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 7833aa1..08aeb7a 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -652,46 +652,48 @@ template bool RobotBodyFilter::configure() { // this->tfFramesWatchdog->start(); // } - // { // initialize the robot body to be masked out - - // std::string robotUrdf; - // while (!this->nodeHandle.getParam(this->robotDescriptionParam, - // robotUrdf) - // || robotUrdf.length() == 0) { - // if (this->failWithoutRobotDescription) - // { - // throw std::runtime_error( - // "RobotBodyFilter: " + this->robotDescriptionParam + " is - // empty or not set."); - // } - // if (!rclcpp::ok()) - // return false; - - // ROS_ERROR("RobotBodyFilter: %s is empty or not set. Please, provide - // the robot model. Waiting 1s.", - // robotDescriptionParam.c_str()); - // rclcpp::Duration(1.0).sleep(); - // } + { // initialize the robot body to be masked out + + std::string robotUrdf; + while (!this->nodeHandle.get_parameter(this->robotDescriptionParam, + robotUrdf) || + robotUrdf.length() == 0) { + if (this->failWithoutRobotDescription) { + throw std::runtime_error( + "RobotBodyFilter: " + this->robotDescriptionParam + + " is empty or not set."); + } + if (!rclcpp::ok()) + return false; - // // happens when configure() is called again from update() (e.g. when - // a new bag file started - // // playing) - // if (!this->shapesToLinks.empty()) - // this->clearRobotMask(); - // this->addRobotMaskFromUrdf(robotUrdf); - // } + RCLCPP_ERROR(this->nodeHandle.get_logger(), + "RobotBodyFilter: %s is empty or not set. Please, provide " + "the robot model. Waiting 1s. ", + robotDescriptionParam.c_str()); + rclcpp::sleep_for(std::chrono::seconds(1)); + } - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Successfully - // configured."); RCLCPP_INFO(nodeHandle.get_logger(),"Filtering data in - // frame %s", this->filteringFrame.c_str()); - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering into - // the following categories:"); - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: \tOUTSIDE"); - // if (doClipping) RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: - // \tCLIP"); if (doContainsTest) - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: \tINSIDE"); if - // (doShadowTest) RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: - // \tSHADOW"); + // happens when configure() is called again from update() (e.g. when + // a new bag file started + // playing) + if (!this->shapesToLinks.empty()) + this->clearRobotMask(); + this->addRobotMaskFromUrdf(robotUrdf); + } + + RCLCPP_INFO(nodeHandle.get_logger(), + "RobotBodyFilter: Successfullyconfigured."); + RCLCPP_INFO(nodeHandle.get_logger(), "Filtering data inframe %s", + this->filteringFrame.c_str()); + RCLCPP_INFO(nodeHandle.get_logger(), + "RobotBodyFilter: Filtering into the following categories:"); + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tOUTSIDE"); + if (doClipping) + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tCLIP"); + if (doContainsTest) + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tINSIDE"); + if (doShadowTest) + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tSHADOW"); // if (this->onlyLinks.empty()) { // if (this->linksIgnoredEverywhere.empty()) { @@ -1419,14 +1421,16 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { // parse the URDF model urdf::Model parsedUrdfModel; bool urdfParseSucceeded = parsedUrdfModel.initString(urdfModel); - if (!urdfParseSucceeded) { - ROS_ERROR_STREAM("RobotBodyFilter: The URDF model given in parameter '" - << this->robotDescriptionParam - << "' cannot be parsed. See " - "urdf::Model::initString for debugging, or try running " - "'gzsdf my_robot.urdf'"); - return; - } + // if (!urdfParseSucceeded) { + // RCLCPP_ERROR(nodeHandle.get_logger(),"RobotBodyFilter: The URDF model + // given in parameter '" + // << this->robotDescriptionParam + // << "' cannot be parsed. See " + // "urdf::Model::initString for debugging, or try + // running " + // "'gzsdf my_robot.urdf'"); + // return; + // } { std::lock_guard guard(*this->modelMutex); From 8d0a37006d3d835e3a010f4a446e18df9785514a Mon Sep 17 00:00:00 2001 From: Tyler Date: Thu, 18 Apr 2024 13:25:52 -0400 Subject: [PATCH 19/54] Formatting (#7) * formatted * sorting includes * headers * formatting comments * comment formatting * comment formatting * brace format * format * format * comments * formatting * Format other files * format includes * ref alignment * formatting * format * format * format * format * Reverted formatting changes --- include/robot_body_filter/RobotBodyFilter.h | 378 ++--- include/robot_body_filter/utils/bodies.h | 20 +- include/robot_body_filter/utils/tf2_eigen.h | 6 +- .../robot_body_filter/utils/time_utils.hpp | 5 +- src/RobotBodyFilter.cpp | 1230 +++++++---------- src/utils/cloud.cpp | 88 +- 6 files changed, 676 insertions(+), 1051 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index a988616..1653b3e 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -4,44 +4,45 @@ #include #include -#include #include -#include -// #include -// #include -#include -#include -// #include -// #include -// #include -// #include -// #include -// #include -#include -#include -#include #include -// #include -#include -// #include // #include #include #include -#include +#include +#include +#include +#include +#include +#include +// #include +// #include #include #include -#include -#include +// #include +// #include +// #include +// #include #include +#include #include +// Remove? +#include +#include +#include +// #include +// #include +// #include + +#include + namespace robot_body_filter { /** - * \brief Just a helper structure holding together a link, one of its collision - * elements, and the index of the collision element in the collision array of - * the link. +* \brief Just a helper structure holding together a link, one of its collision elements, + * and the index of the collision element in the collision array of the link. */ struct CollisionBodyWithLink { urdf::CollisionSharedPtr collision; @@ -50,40 +51,40 @@ struct CollisionBodyWithLink { MultiShapeHandle multiHandle; std::string cacheKey; - CollisionBodyWithLink() : indexInCollisionArray(0), cacheKey("__empty__") {} + CollisionBodyWithLink() : + indexInCollisionArray(0), cacheKey("__empty__") { + } CollisionBodyWithLink(urdf::CollisionSharedPtr collision, urdf::LinkSharedPtr link, const size_t indexInCollisionArray, - const MultiShapeHandle &multiHandle) - : collision(collision), link(link), - indexInCollisionArray(indexInCollisionArray), multiHandle(multiHandle) { + const MultiShapeHandle& multiHandle): + collision(collision), link(link), indexInCollisionArray(indexInCollisionArray), + multiHandle(multiHandle) + { std::ostringstream stream; stream << link->name << "-" << indexInCollisionArray; this->cacheKey = stream.str(); } }; -struct ScaleAndPadding { +struct ScaleAndPadding +{ double scale; double padding; ScaleAndPadding(double scale = 1.0, double padding = 0.0); - bool operator==(const ScaleAndPadding &other) const; - bool operator!=(const ScaleAndPadding &other) const; + bool operator==(const ScaleAndPadding& other) const; + bool operator!=(const ScaleAndPadding& other) const; }; -/** \brief Suffix added to link/collision names to distinguish their usage in - * contains tests only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in contains tests only. */ static const std::string CONTAINS_SUFFIX = "::contains"; -/** \brief Suffix added to link/collision names to distinguish their usage in - * shadow tests only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in shadow tests only. */ static const std::string SHADOW_SUFFIX = "::shadow"; -/** \brief Suffix added to link/collision names to distinguish their usage in - * bounding sphere computation only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in bounding sphere computation only. */ static const std::string BSPHERE_SUFFIX = "::bounding_sphere"; -/** \brief Suffix added to link/collision names to distinguish their usage in - * bounding box computation only. */ +/** \brief Suffix added to link/collision names to distinguish their usage in bounding box computation only. */ static const std::string BBOX_SUFFIX = "::bounding_box"; /** @@ -93,7 +94,8 @@ static const std::string BBOX_SUFFIX = "::bounding_box"; * * \author Martin Pecka */ -template class RobotBodyFilter : public filters::FilterBase { +template +class RobotBodyFilter : public filters::FilterBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -104,7 +106,7 @@ template class RobotBodyFilter : public filters::FilterBase { //! Parameters are described in the readme. bool configure() override; - bool update(const T &data_in, T &data_out) override = 0; + bool update(const T& data_in, T& data_out) override = 0; protected: //! Handle of the node this filter runs in. @@ -156,56 +158,45 @@ template class RobotBodyFilter : public filters::FilterBase { //! The minimum distance of points from the sensor to keep them (in meters). double minDistance; - //! The maximum distance of points from the sensor origin to apply this filter - //! on (in meters). + //! The maximum distance of points from the sensor origin to apply this filter on (in meters). double maxDistance; - //! The default inflation that is applied to the collision model for the - //! purposes of checking if a point is contained by the robot model (scale 1.0 - //! = no scaling, padding 0.0 = no padding). Every collision element is scaled - //! individually with the scaling center in its origin. Padding is added - //! individually to every collision element. + //! The default inflation that is applied to the collision model for the purposes of checking if a point is contained + //! by the robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Every collision element is scaled + //! individually with the scaling center in its origin. Padding is added individually to every collision element. ScaleAndPadding defaultContainsInflation; - //! The default inflation that is applied to the collision model for the - //! purposes of checking if a point is shadowed by the robot model (scale 1.0 - //! = no scaling, padding 0.0 = no padding). Every collision element is scaled - //! individually with the scaling center in its origin. Padding is added - //! individually to every collision element. + //! The default inflation that is applied to the collision model for the purposes of checking if a point is shadowed + //! by the robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Every collision element is scaled + //! individually with the scaling center in its origin. Padding is added individually to every collision element. ScaleAndPadding defaultShadowInflation; - //! The default inflation that is applied to the collision model for the - //! purposes of computing the bounding sphere. Every collision element is - //! scaled individually with the scaling center in its origin. Padding is - //! added individually to every collision element. + //! The default inflation that is applied to the collision model for the purposes of computing the bounding sphere. + //! Every collision element is scaled individually with the scaling center in its origin. Padding is added + //! individually to every collision element. ScaleAndPadding defaultBsphereInflation; - //! The default inflation that is applied to the collision model for the - //! purposes of computing the bounding box. Every collision element is scaled - //! individually with the scaling center in its origin. Padding is added + //! The default inflation that is applied to the collision model for the purposes of computing the bounding box. + //! Every collision element is scaled individually with the scaling center in its origin. Padding is added //! individually to every collision element. ScaleAndPadding defaultBboxInflation; - //! Inflation that is applied to a collision element for the purposes of - //! checking if a point is contained by the robot model (scale 1.0 = no - //! scaling, padding 0.0 = no padding). Elements not present in this list are - //! scaled and padded with defaultContainsInflation. + //! Inflation that is applied to a collision element for the purposes of checking if a point is contained by the + //! robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Elements not present in this list are scaled and + //! padded with defaultContainsInflation. std::map perLinkContainsInflation; - //! Inflation that is applied to a collision element for the purposes of - //! checking if a point is shadowed by the robot model (scale 1.0 = no - //! scaling, padding 0.0 = no padding). Elements not present in this list are - //! scaled and padded with defaultShadowInflation. + //! Inflation that is applied to a collision element for the purposes of checking if a point is shadowed by the + //! robot model (scale 1.0 = no scaling, padding 0.0 = no padding). Elements not present in this list are scaled and + //! padded with defaultShadowInflation. std::map perLinkShadowInflation; - //! Inflation that is applied to a collision element for the purposes of - //! computing the bounding sphere. Elements not present in this list are - //! scaled and padded with defaultBsphereInflation. + //! Inflation that is applied to a collision element for the purposes of computing the bounding sphere. + //! Elements not present in this list are scaled and padded with defaultBsphereInflation. std::map perLinkBsphereInflation; - //! Inflation that is applied to a collision element for the purposes of - //! computing the bounding box. Elements not present in this list are scaled - //! and padded with defaultBboxInflation. + //! Inflation that is applied to a collision element for the purposes of computing the bounding box. + //! Elements not present in this list are scaled and padded with defaultBboxInflation. std::map perLinkBboxInflation; //! Name of the parameter where the robot model can be found. @@ -213,8 +204,8 @@ template class RobotBodyFilter : public filters::FilterBase { //! Subscriber for robot_description updates. // ros::Subscriber robotDescriptionUpdatesListener; - //! Name of the field in the dynamic reconfigure message that contains robot - //! model. + + //! Name of the field in the dynamic reconfigure message that contains robot model. std::string robotDescriptionUpdatesFieldName; std::set linksIgnoredInBoundingSphere; @@ -226,72 +217,48 @@ template class RobotBodyFilter : public filters::FilterBase { //! Publisher of robot bounding sphere (relative to fixed frame). // ros::Publisher boundingSpherePublisher; - // They use a custom msg type, we can just use an unstamped std shape_msg for - // simplicity - rclcpp::Publisher::SharedPtr - boundingSpherePublisher; + // They use a custom msg type, we can just use an unstamped std shape_msg for simplicity + rclcpp::Publisher::SharedPtr boundingSpherePublisher; // //! Publisher of robot bounding box (relative to fixed frame). - rclcpp::Publisher::SharedPtr - boundingBoxPublisher; + rclcpp::Publisher::SharedPtr boundingBoxPublisher; // //! Publisher of robot bounding box (relative to fixed frame). - rclcpp::Publisher::SharedPtr - orientedBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr orientedBoundingBoxPublisher; // //! Publisher of robot bounding box (relative to defined local frame). - rclcpp::Publisher::SharedPtr - localBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr localBoundingBoxPublisher; // //! Publisher of the bounding sphere marker. - rclcpp::Publisher::SharedPtr - boundingSphereMarkerPublisher; + rclcpp::Publisher::SharedPtr boundingSphereMarkerPublisher; // //! Publisher of the bounding box marker. - rclcpp::Publisher::SharedPtr - boundingBoxMarkerPublisher; + rclcpp::Publisher::SharedPtr boundingBoxMarkerPublisher; // //! Publisher of the oriented bounding box marker. - rclcpp::Publisher::SharedPtr - orientedBoundingBoxMarkerPublisher; + rclcpp::Publisher::SharedPtr orientedBoundingBoxMarkerPublisher; //! Publisher of the local bounding box marker. - rclcpp::Publisher::SharedPtr - localBoundingBoxMarkerPublisher; + rclcpp::Publisher::SharedPtr localBoundingBoxMarkerPublisher; //! Publisher of the debug bounding box markers. - rclcpp::Publisher::SharedPtr - boundingBoxDebugMarkerPublisher; + rclcpp::Publisher::SharedPtr boundingBoxDebugMarkerPublisher; //! Publisher of the debug oriented bounding box markers. - rclcpp::Publisher::SharedPtr - orientedBoundingBoxDebugMarkerPublisher; + rclcpp::Publisher::SharedPtr orientedBoundingBoxDebugMarkerPublisher; //! Publisher of the debug local bounding box markers. - rclcpp::Publisher::SharedPtr - localBoundingBoxDebugMarkerPublisher; + rclcpp::Publisher::SharedPtr localBoundingBoxDebugMarkerPublisher; //! Publisher of the debug bounding sphere markers. - rclcpp::Publisher::SharedPtr - boundingSphereDebugMarkerPublisher; + rclcpp::Publisher::SharedPtr boundingSphereDebugMarkerPublisher; //! Publisher of scan_point_cloud with robot bounding box cut out. - rclcpp::Publisher::SharedPtr - scanPointCloudNoBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr scanPointCloudNoBoundingBoxPublisher; //! Publisher of scan_point_cloud with robot oriented bounding box cut out. - rclcpp::Publisher::SharedPtr - scanPointCloudNoOrientedBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr scanPointCloudNoOrientedBoundingBoxPublisher; //! Publisher of scan_point_cloud with robot local bounding box cut out. - rclcpp::Publisher::SharedPtr - scanPointCloudNoLocalBoundingBoxPublisher; + rclcpp::Publisher::SharedPtr scanPointCloudNoLocalBoundingBoxPublisher; //! Publisher of scan_point_cloud with robot bounding sphere cut out. - rclcpp::Publisher::SharedPtr - scanPointCloudNoBoundingSpherePublisher; - rclcpp::Publisher::SharedPtr - debugPointCloudInsidePublisher; - rclcpp::Publisher::SharedPtr - debugPointCloudClipPublisher; - rclcpp::Publisher::SharedPtr - debugPointCloudShadowPublisher; - rclcpp::Publisher::SharedPtr - debugContainsMarkerPublisher; - rclcpp::Publisher::SharedPtr - debugShadowMarkerPublisher; - rclcpp::Publisher::SharedPtr - debugBsphereMarkerPublisher; - rclcpp::Publisher::SharedPtr - debugBboxMarkerPublisher; - - // ! Service server for reloading robot model. + rclcpp::Publisher::SharedPtr scanPointCloudNoBoundingSpherePublisher; + rclcpp::Publisher::SharedPtr debugPointCloudInsidePublisher; + rclcpp::Publisher::SharedPtr debugPointCloudClipPublisher; + rclcpp::Publisher::SharedPtr debugPointCloudShadowPublisher; + rclcpp::Publisher::SharedPtr debugContainsMarkerPublisher; + rclcpp::Publisher::SharedPtr debugShadowMarkerPublisher; + rclcpp::Publisher::SharedPtr debugBsphereMarkerPublisher; + rclcpp::Publisher::SharedPtr debugBboxMarkerPublisher; + + //! Service server for reloading robot model. // ros::ServiceServer reloadRobotModelServiceServer; //! Whether to compute bounding sphere of the robot. @@ -320,8 +287,7 @@ template class RobotBodyFilter : public filters::FilterBase { bool publishBoundingSphereMarker; //! Whether to publish scan_point_cloud with robot bounding box cut out. bool publishNoBoundingBoxPointcloud; - //! Whether to publish scan_point_cloud with robot oriented bounding box cut - //! out. + //! Whether to publish scan_point_cloud with robot oriented bounding box cut out. bool publishNoOrientedBoundingBoxPointcloud; //! Whether to publish scan_point_cloud with robot local bounding box cut out. bool publishNoLocalBoundingBoxPointcloud; @@ -347,8 +313,7 @@ template class RobotBodyFilter : public filters::FilterBase { //! Whether to process data when there are some unreachable frames. bool requireAllFramesReachable; - //! A mutex that has to be locked in order to work with shapesToLinks or - //! tfBuffer. + //! A mutex that has to be locked in order to work with shapesToLinks or tfBuffer. std::shared_ptr modelMutex; //! tf buffer length @@ -367,31 +332,23 @@ template class RobotBodyFilter : public filters::FilterBase { //! Tool for masking out 3D bodies out of point clouds. std::unique_ptr shapeMask; - /// A map that correlates shapes in robot_shape_mask to collision links in - /// URDF. + /// A map that correlates shapes in robot_shape_mask to collision links in URDF. /** Keys are shape handles from shapeMask. */ - std::map - shapesToLinks; + std::map shapesToLinks; std::set shapesIgnoredInBoundingSphere; std::set shapesIgnoredInBoundingBox; - //! Caches any link->fixedFrame transforms after a scan message is received. - //! Is queried by robot_shape_mask. Keys are CollisionBodyWithLink#cacheKey. - std::map> transformCache; - //! Caches any link->fixedFrame transforms at the time of scan end. Only used - //! for pointByPoint scans. Is queried by robot_shape_mask. Keys are - //! CollisionBodyWithLink#cacheKey. - std::map> - transformCacheAfterScan; - - //! If the scan is pointByPoint, set this variable to the ratio between scan - //! start and end time you're looking for with getShapeTransform(). + //! Caches any link->fixedFrame transforms after a scan message is received. Is queried by robot_shape_mask. Keys are CollisionBodyWithLink#cacheKey. + std::map > transformCache; + //! Caches any link->fixedFrame transforms at the time of scan end. Only used for pointByPoint scans. Is queried by robot_shape_mask. Keys are CollisionBodyWithLink#cacheKey. + std::map > transformCacheAfterScan; + + //! If the scan is pointByPoint, set this variable to the ratio between scan start and end time you're looking for with getShapeTransform(). mutable double cacheLookupBetweenScansRatio; - //! Used in tests. If false, configure() waits until robot description becomes - //! available. If true, configure() fails with std::runtime_exception if robot - //! description is not available. + //! Used in tests. If false, configure() waits until robot description becomes available. If true, + //! configure() fails with std::runtime_exception if robot description is not available. bool failWithoutRobotDescription = false; /** @@ -408,32 +365,28 @@ template class RobotBodyFilter : public filters::FilterBase { * sensor position from the viewpoint channels. * \return Whether the computation succeeded. */ - bool computeMask(const sensor_msgs::msg::PointCloud2 &projectedPointCloud, - std::vector &mask, - const std::string &sensorFrame = ""); + bool computeMask(const sensor_msgs::msg::PointCloud2& projectedPointCloud, + std::vector& mask, + const std::string& sensorFrame = ""); - /** \brief Return the latest cached transform for the link corresponding to - * the given shape handle. + /** \brief Return the latest cached transform for the link corresponding to the given shape handle. * * You should call updateTransformCache before calling this function. * - * \param shapeHandle The handle of the shape for which we want the transform. - * The handle is from robot_shape_mask. \param[out] transform Transform of the - * corresponding link (wrt filtering frame). \return If the transform was - * found. + * \param shapeHandle The handle of the shape for which we want the transform. The handle is from robot_shape_mask. + * \param[out] transform Transform of the corresponding link (wrt filtering frame). + * \return If the transform was found. */ - bool getShapeTransform(point_containment_filter::ShapeHandle shapeHandle, - Eigen::Isometry3d &transform) const; + bool getShapeTransform(point_containment_filter::ShapeHandle shapeHandle, Eigen::Isometry3d& transform) const; /** \brief Update robot_shape_mask with the given URDF model. * * \param urdfModel The robot's URDF loaded as a string. */ - void addRobotMaskFromUrdf(const std::string &urdfModel); + void addRobotMaskFromUrdf(const std::string& urdfModel); /** - * \brief Remove all parts of the robot mask and clear internal shape and TF - * buffers. + * \brief Remove all parts of the robot mask and clear internal shape and TF buffers. * * Make sure no filtering happens when executing this function. */ @@ -442,16 +395,12 @@ template class RobotBodyFilter : public filters::FilterBase { /** \brief Update the cache of link transforms relative to filtering frame. * * \param time The time to get transforms for. - * \param afterScantime The after scan time to get transforms for (if zero - * time is passed, after scan transforms are not computed). + * \param afterScantime The after scan time to get transforms for (if zero time is passed, after scan transforms are not computed). */ - void - updateTransformCache(const rclcpp::Time &time, - const rclcpp::Time &afterScanTime = rclcpp::Time(0)); + void updateTransformCache(const rclcpp::Time& time, const rclcpp::Time& afterScanTime = rclcpp::Time(0)); /** - * \brief Callback handling update of the robot_description parameter using - * dynamic reconfigure. + * \brief Callback handling update of the robot_description parameter using dynamic reconfigure. * * \param newConfig The updated config. */ @@ -462,95 +411,74 @@ template class RobotBodyFilter : public filters::FilterBase { * \brief Callback for ~reload_model service. Reloads the URDF from parameter. * \return Success. */ - bool triggerModelReload(std_srvs::srv::Trigger_Request &, - std_srvs::srv::Trigger_Response &); + bool triggerModelReload(std_srvs::srv::Trigger_Request&, std_srvs::srv::Trigger_Response&); void createBodyVisualizationMsg( - const std::map &bodies, - const rclcpp::Time &stamp, const std_msgs::msg::ColorRGBA &color, - visualization_msgs::msg::MarkerArray &markerArray) const; + const std::map& bodies, + const rclcpp::Time& stamp, const std_msgs::msg::ColorRGBA& color, + visualization_msgs::msg::MarkerArray& markerArray) const; - void publishDebugMarkers(const rclcpp::Time &scanTime) const; + void publishDebugMarkers(const rclcpp::Time& scanTime) const; void publishDebugPointClouds( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud, - const std::vector &pointMask) const; + const sensor_msgs::msg::PointCloud2& projectedPointCloud, + const std::vector& pointMask) const; /** * \brief Computation of the bounding sphere, debug spheres, and publishing of * pointcloud without bounding sphere. */ - void computeAndPublishBoundingSphere( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; + void computeAndPublishBoundingSphere(const sensor_msgs::msg::PointCloud2& projectedPointCloud) const; /** * \brief Computation of the bounding box, debug boxes, and publishing of * pointcloud without bounding box. */ - void computeAndPublishBoundingBox( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; + void computeAndPublishBoundingBox(const sensor_msgs::msg::PointCloud2& projectedPointCloud) const; /** - * \brief Computation of the oriented bounding box, debug boxes, and - * publishing of pointcloud without bounding box. + * \brief Computation of the oriented bounding box, debug boxes, and publishing of + * pointcloud without bounding box. */ - void computeAndPublishOrientedBoundingBox( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; + void computeAndPublishOrientedBoundingBox(const sensor_msgs::msg::PointCloud2& projectedPointCloud) const; /** - * \brief Computation of the local bounding box, debug boxes, and publishing - * of pointcloud without bounding box. + * \brief Computation of the local bounding box, debug boxes, and publishing of + * pointcloud without bounding box. */ - void computeAndPublishLocalBoundingBox( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const; - - ScaleAndPadding - getLinkInflationForContainsTest(const std::string &linkName) const; - ScaleAndPadding getLinkInflationForContainsTest( - const std::vector &linkNames) const; - ScaleAndPadding - getLinkInflationForShadowTest(const std::string &linkName) const; - ScaleAndPadding getLinkInflationForShadowTest( - const std::vector &linkNames) const; - ScaleAndPadding - getLinkInflationForBoundingSphere(const std::string &linkName) const; - ScaleAndPadding getLinkInflationForBoundingSphere( - const std::vector &linkNames) const; - ScaleAndPadding - getLinkInflationForBoundingBox(const std::string &linkName) const; - ScaleAndPadding getLinkInflationForBoundingBox( - const std::vector &linkNames) const; + void computeAndPublishLocalBoundingBox(const sensor_msgs::msg::PointCloud2& projectedPointCloud) const; + + ScaleAndPadding getLinkInflationForContainsTest(const std::string& linkName) const; + ScaleAndPadding getLinkInflationForContainsTest(const std::vector& linkNames) const; + ScaleAndPadding getLinkInflationForShadowTest(const std::string& linkName) const; + ScaleAndPadding getLinkInflationForShadowTest(const std::vector& linkNames) const; + ScaleAndPadding getLinkInflationForBoundingSphere(const std::string& linkName) const; + ScaleAndPadding getLinkInflationForBoundingSphere(const std::vector& linkNames) const; + ScaleAndPadding getLinkInflationForBoundingBox(const std::string& linkName) const; + ScaleAndPadding getLinkInflationForBoundingBox(const std::vector& linkNames) const; private: - ScaleAndPadding getLinkInflation( - const std::vector &linkNames, - const ScaleAndPadding &defaultInflation, - const std::map &perLinkInflation) const; + ScaleAndPadding getLinkInflation(const std::vector& linkNames, const ScaleAndPadding& defaultInflation, const std::map& perLinkInflation) const; }; -class RobotBodyFilterLaserScan - : public RobotBodyFilter { +class RobotBodyFilterLaserScan : public RobotBodyFilter +{ public: //! Apply the filter. - bool update(const sensor_msgs::msg::LaserScan &inputScan, - sensor_msgs::msg::LaserScan &filteredScan) override; + bool update(const sensor_msgs::msg::LaserScan& inputScan, sensor_msgs::msg::LaserScan& filteredScan) override; bool configure() override; protected: laser_geometry::LaserProjection laserProjector; - // in RobotBodyFilterLaserScan::update we project the scan to a pointcloud - // with viewpoints - const std::unordered_map channelsToTransform{ - {"vp_", CloudChannelType::POINT}}; + // in RobotBodyFilterLaserScan::update we project the scan to a pointcloud with viewpoints + const std::unordered_map channelsToTransform { {"vp_", CloudChannelType::POINT} }; }; -class RobotBodyFilterPointCloud2 - : public RobotBodyFilter { +class RobotBodyFilterPointCloud2 : public RobotBodyFilter +{ public: //! Apply the filter. - bool update(const sensor_msgs::msg::PointCloud2 &inputCloud, - sensor_msgs::msg::PointCloud2 &filteredCloud) override; + bool update(const sensor_msgs::msg::PointCloud2& inputCloud, sensor_msgs::msg::PointCloud2& filteredCloud) override; bool configure() override; @@ -561,6 +489,6 @@ class RobotBodyFilterPointCloud2 std::unordered_map channelsToTransform; }; -} // namespace robot_body_filter +} -#endif // ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ +#endif //ROBOT_BODY_FILTER_ROBOTSELFFILTER_H_ diff --git a/include/robot_body_filter/utils/bodies.h b/include/robot_body_filter/utils/bodies.h index 1bee703..9085e1f 100644 --- a/include/robot_body_filter/utils/bodies.h +++ b/include/robot_body_filter/utils/bodies.h @@ -1,30 +1,28 @@ #ifndef ROBOT_BODY_FILTER_BODIES_H #define ROBOT_BODY_FILTER_BODIES_H -#include "obb.hpp" -#include #include +#include #if __has_include() #include #else +#include "obb.hpp" // #error not found. Please, update geometric_shapes // library to version 0.6.5+ or 0.7.4+ #endif -// #include +#include -namespace bodies { +namespace bodies +{ typedef bodies::AABB AxisAlignedBoundingBox; typedef bodies::OBB OrientedBoundingBox; -/** \brief Compute AABB for the body at different pose. Can't use setPose() - * because we want `body` to be const. */ -void computeBoundingBoxAt(const bodies::Body *body, - AxisAlignedBoundingBox &bbox, - const Eigen::Isometry3d &pose); +/** \brief Compute AABB for the body at different pose. Can't use setPose() because we want `body` to be const. */ +void computeBoundingBoxAt(const bodies::Body* body, AxisAlignedBoundingBox& bbox, const Eigen::Isometry3d& pose); -} // namespace bodies +} -#endif // ROBOT_BODY_FILTER_BODIES_H +#endif //ROBOT_BODY_FILTER_BODIES_H diff --git a/include/robot_body_filter/utils/tf2_eigen.h b/include/robot_body_filter/utils/tf2_eigen.h index 040abeb..3d99ab1 100644 --- a/include/robot_body_filter/utils/tf2_eigen.h +++ b/include/robot_body_filter/utils/tf2_eigen.h @@ -2,10 +2,10 @@ #define ROBOT_BODY_FILTER_TF2_EIGEN_H #include -#include +#include namespace tf2 { -void toMsg(const Eigen::Vector3d &in, geometry_msgs::msg::Point32 &out); +void toMsg(const Eigen::Vector3d& in, geometry_msgs::msg::Point32& out); } -#endif // ROBOT_BODY_FILTER_TF2_EIGEN_H +#endif //ROBOT_BODY_FILTER_TF2_EIGEN_H diff --git a/include/robot_body_filter/utils/time_utils.hpp b/include/robot_body_filter/utils/time_utils.hpp index 278547d..b6f20f5 100644 --- a/include/robot_body_filter/utils/time_utils.hpp +++ b/include/robot_body_filter/utils/time_utils.hpp @@ -1,7 +1,6 @@ #ifndef ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP #define ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP -// #include #include namespace robot_body_filter { @@ -25,6 +24,6 @@ rclcpp::Duration remainingTime(const rclcpp::Time &query, double timeout); rclcpp::Duration remainingTime(const rclcpp::Time &query, const rclcpp::Duration &timeout); -}; // namespace robot_body_filter +}; -#endif // ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP \ No newline at end of file +#endif //ROBOT_BODY_FILTER_UTILS_TIME_UTILS_HPP \ No newline at end of file diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 08aeb7a..9a51b0a 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1,8 +1,8 @@ -#include -#include -#include #include +#include +#include + #include "robot_body_filter/RobotBodyFilter.h" // #include "pluginlib/class_list_macros.h" @@ -12,13 +12,13 @@ #include #include -#include #include +#include #include #include -#include #include +#include #include #include @@ -30,6 +30,7 @@ #include #include +#include // using namespace std; // using namespace sensor_msgs; // using namespace filters; @@ -38,9 +39,12 @@ namespace robot_body_filter { template RobotBodyFilter::RobotBodyFilter() - : privateNodeHandle("~"), nodeHandle("robot_body_filter"), - modelPoseUpdateInterval(0, 0), reachableTransformTimeout(0, 0), - unreachableTransformTimeout(0, 0), tfBufferLength(0, 0) { + : privateNodeHandle("~"), + nodeHandle("robot_body_filter"), + modelPoseUpdateInterval(0, 0), + reachableTransformTimeout(0, 0), + unreachableTransformTimeout(0, 0), + tfBufferLength(0, 0) { this->modelMutex.reset(new std::mutex()); // Declare ROS2 Parameters in node constructor @@ -51,8 +55,7 @@ RobotBodyFilter::RobotBodyFilter() param_desc.description = "frames/sensor"; this->nodeHandle.declare_parameter("sensorFrame", "", param_desc); param_desc.description = "frames/filtering"; - this->nodeHandle.declare_parameter("filteringFrame", this->fixedFrame, - param_desc); + this->nodeHandle.declare_parameter("filteringFrame", this->fixedFrame, param_desc); param_desc.description = "m"; param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; this->nodeHandle.declare_parameter("minDistance", 0.0, param_desc); @@ -61,28 +64,20 @@ RobotBodyFilter::RobotBodyFilter() this->nodeHandle.declare_parameter("robot_description", "", param_desc); this->nodeHandle.declare_parameter("filter/keep_clouds_organized", true); param_desc.description = "s"; - this->nodeHandle.declare_parameter("filter/model_pose_update_interval", 0.0, - param_desc); + this->nodeHandle.declare_parameter("filter/model_pose_update_interval", 0.0, param_desc); this->nodeHandle.declare_parameter("filter/do_clipping", true); this->nodeHandle.declare_parameter("filter/do_contains_test", true); this->nodeHandle.declare_parameter("filter/do_shadow_test", true); param_desc.description = "m"; - this->nodeHandle.declare_parameter("filter/max_shadow_distance", - this->maxDistance, param_desc); + this->nodeHandle.declare_parameter("filter/max_shadow_distance", this->maxDistance, param_desc); param_desc.description = "s"; - this->nodeHandle.declare_parameter("transforms/timeout/reachable", 0.1, - param_desc); - this->nodeHandle.declare_parameter("transforms/timeout/unreachable", 0.2, - param_desc); + this->nodeHandle.declare_parameter("transforms/timeout/reachable", 0.1, param_desc); + this->nodeHandle.declare_parameter("transforms/timeout/unreachable", 0.2, param_desc); this->nodeHandle.declare_parameter("transforms/require_all_reachable", false); - this->nodeHandle.declare_parameter( - "bounding_sphere/publish_cut_out_pointcloud", false); - this->nodeHandle.declare_parameter("bounding_box/publish_cut_out_pointcloud", - false); - this->nodeHandle.declare_parameter( - "oriented_bounding_box/publish_cut_out_pointcloud", false); - this->nodeHandle.declare_parameter( - "local_bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter("bounding_sphere/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter("bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter("oriented_bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle.declare_parameter("local_bounding_box/publish_cut_out_pointcloud", false); this->nodeHandle.declare_parameter("bounding_sphere/compute", false); this->nodeHandle.declare_parameter("bounding_box/compute", false); this->nodeHandle.declare_parameter("oriented_bounding_box/compute", false); @@ -95,8 +90,7 @@ RobotBodyFilter::RobotBodyFilter() this->nodeHandle.declare_parameter("bounding_box/marker", false); this->nodeHandle.declare_parameter("oriented_bounding_box/marker", false); this->nodeHandle.declare_parameter("local_bounding_box/marker", false); - this->nodeHandle.declare_parameter("local_bounding_box/frame_id", - this->fixedFrame); + this->nodeHandle.declare_parameter("local_bounding_box/frame_id", this->fixedFrame); this->nodeHandle.declare_parameter("debug/pcl/inside", false); this->nodeHandle.declare_parameter("debug/pcl/clip", false); this->nodeHandle.declare_parameter("debug/pcl/shadow", false); @@ -105,36 +99,25 @@ RobotBodyFilter::RobotBodyFilter() this->nodeHandle.declare_parameter("debug/marker/bounding_sphere", false); this->nodeHandle.declare_parameter("debug/marker/bounding_box", false); param_desc.description = "m"; - this->nodeHandle.declare_parameter("body_model/inflation/padding", 0.0, - param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/padding", 0.0, param_desc); this->nodeHandle.declare_parameter("body_model/inflation/scale", 1.0); // NOTE: Default changed from inflationPadding/inflationScale to 0.0/1.0 - this->nodeHandle.declare_parameter( - "body_model/inflation/contains_test/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/contains_test/scale", - 1.0); - this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/padding", - 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/scale", - 1.0); - this->nodeHandle.declare_parameter( - "body_model/inflation/bounding_sphere/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter( - "body_model/inflation/bounding_sphere/scale", 1.0); - this->nodeHandle.declare_parameter( - "body_model/inflation/bounding_box/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/scale", - 1.0); + this->nodeHandle.declare_parameter("body_model/inflation/contains_test/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/contains_test/scale", 1.0); + this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/scale", 1.0); + this->nodeHandle.declare_parameter("body_model/inflation/bounding_sphere/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/bounding_sphere/scale", 1.0); + this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/padding", 0.0, param_desc); + this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/scale", 1.0); // TODO: This initialization may be incorrect, not sure if I understand how // this works // https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html#ae5ab12777100f65bd09163814dbbf486 // this might need to be initialized with the names of each link - this->nodeHandle.declare_parameters("body_model/inflation/per_link/padding", - std::map()); + this->nodeHandle.declare_parameters("body_model/inflation/per_link/padding", std::map()); - this->nodeHandle.declare_parameters("body_model/inflation/per_link/scale", - std::map()); + this->nodeHandle.declare_parameters("body_model/inflation/per_link/scale", std::map()); // Note: some of these are init to empty strings, originally they are vectors // of strings @@ -144,19 +127,17 @@ RobotBodyFilter::RobotBodyFilter() this->nodeHandle.declare_parameter("ignored_links/shadow_test", "laser"); this->nodeHandle.declare_parameter("ignored_links/everywhere", ""); this->nodeHandle.declare_parameter("only_links", ""); - this->nodeHandle.declare_parameter( - "body_model/dynamic_robot_description/field_name", "robot_model"); + this->nodeHandle.declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model"); } -template bool RobotBodyFilter::configure() { +template +bool RobotBodyFilter::configure() { // this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", // rclcpp::Duration(60.0), "s"); if (this->tfBuffer == nullptr) { tf2::Duration tf2_duration = tf2_ros::fromRclcpp(this->tfBufferLength); - this->tfBuffer = - std::make_shared(nodeHandle.get_clock(), tf2_duration); - this->tfListener = - std::make_unique(*this->tfBuffer); + this->tfBuffer = std::make_shared(nodeHandle.get_clock(), tf2_duration); + this->tfListener = std::make_unique(*this->tfBuffer); } else { // clear the TF buffer (useful if calling configure() after receiving old TF // data) @@ -171,16 +152,12 @@ template bool RobotBodyFilter::configure() { stripLeadingSlash(this->sensorFrame, true); this->nodeHandle.get_parameter("minDistance", this->minDistance); this->nodeHandle.get_parameter("maxDistance", this->maxDistance); - this->nodeHandle.get_parameter("body_model/robot_description_param", - this->robotDescriptionParam); - this->nodeHandle.get_parameter("filter/keep_clouds_organized", - this->keepCloudsOrganized); + this->nodeHandle.get_parameter("body_model/robot_description_param", this->robotDescriptionParam); + this->nodeHandle.get_parameter("filter/keep_clouds_organized", this->keepCloudsOrganized); double tempModelPoseUpdateInterval; - this->nodeHandle.get_parameter("filter/model_pose_update_interval", - tempModelPoseUpdateInterval); - this->modelPoseUpdateInterval = - rclcpp::Duration::from_seconds(tempModelPoseUpdateInterval); + this->nodeHandle.get_parameter("filter/model_pose_update_interval", tempModelPoseUpdateInterval); + this->modelPoseUpdateInterval = rclcpp::Duration::from_seconds(tempModelPoseUpdateInterval); bool tempDoClipping; this->nodeHandle.get_parameter("filter/do_clipping", tempDoClipping); @@ -195,147 +172,102 @@ template bool RobotBodyFilter::configure() { const bool doShadowTest = tempDoShadowTest; double tempMaxShadowDistance; - this->nodeHandle.get_parameter("filter/max_shadow_distance", - tempMaxShadowDistance); + this->nodeHandle.get_parameter("filter/max_shadow_distance", tempMaxShadowDistance); const double maxShadowDistance = tempMaxShadowDistance; double tempReachableTransformTimeout; - this->nodeHandle.get_parameter("transforms/timeout/reachable", - tempReachableTransformTimeout); - this->reachableTransformTimeout = - rclcpp::Duration::from_seconds(tempReachableTransformTimeout); + this->nodeHandle.get_parameter("transforms/timeout/reachable", tempReachableTransformTimeout); + this->reachableTransformTimeout = rclcpp::Duration::from_seconds(tempReachableTransformTimeout); double tempUnreachableTransformTimeout; - this->nodeHandle.get_parameter("transforms/timeout/unreachable", - tempUnreachableTransformTimeout); - this->unreachableTransformTimeout = - rclcpp::Duration::from_seconds(tempUnreachableTransformTimeout); + this->nodeHandle.get_parameter("transforms/timeout/unreachable", tempUnreachableTransformTimeout); + this->unreachableTransformTimeout = rclcpp::Duration::from_seconds(tempUnreachableTransformTimeout); - this->nodeHandle.get_parameter("transforms/require_all_reachable", - this->requireAllFramesReachable); + this->nodeHandle.get_parameter("transforms/require_all_reachable", this->requireAllFramesReachable); - this->nodeHandle.get_parameter("bounding_sphere/publish_cut_out_pointcloud", - this->publishNoBoundingSpherePointcloud); + this->nodeHandle.get_parameter("bounding_sphere/publish_cut_out_pointcloud", this->publishNoBoundingSpherePointcloud); - this->nodeHandle.get_parameter("bounding_box/publish_cut_out_pointcloud", - this->publishNoBoundingBoxPointcloud); + this->nodeHandle.get_parameter("bounding_box/publish_cut_out_pointcloud", this->publishNoBoundingBoxPointcloud); - this->nodeHandle.get_parameter( - "oriented_bounding_box/publish_cut_out_pointcloud", - this->publishNoOrientedBoundingBoxPointcloud); + this->nodeHandle.get_parameter("oriented_bounding_box/publish_cut_out_pointcloud", + this->publishNoOrientedBoundingBoxPointcloud); - this->nodeHandle.get_parameter( - "local_bounding_box/publish_cut_out_pointcloud", - this->publishNoLocalBoundingBoxPointcloud); + this->nodeHandle.get_parameter("local_bounding_box/publish_cut_out_pointcloud", + this->publishNoLocalBoundingBoxPointcloud); - this->nodeHandle.get_parameter("bounding_sphere/compute", - this->computeBoundingSphere); - this->computeBoundingSphere = - this->computeBoundingSphere || this->publishNoBoundingSpherePointcloud; + this->nodeHandle.get_parameter("bounding_sphere/compute", this->computeBoundingSphere); + this->computeBoundingSphere = this->computeBoundingSphere || this->publishNoBoundingSpherePointcloud; - this->nodeHandle.get_parameter("bounding_box/compute", - this->computeBoundingBox); - this->computeBoundingBox = - this->computeBoundingBox || this->publishNoBoundingBoxPointcloud; + this->nodeHandle.get_parameter("bounding_box/compute", this->computeBoundingBox); + this->computeBoundingBox = this->computeBoundingBox || this->publishNoBoundingBoxPointcloud; - this->nodeHandle.get_parameter("oriented_bounding_box/compute", - this->computeOrientedBoundingBox); - this->computeOrientedBoundingBox = - this->computeOrientedBoundingBox || - this->publishNoOrientedBoundingBoxPointcloud; + this->nodeHandle.get_parameter("oriented_bounding_box/compute", this->computeOrientedBoundingBox); + this->computeOrientedBoundingBox = this->computeOrientedBoundingBox || this->publishNoOrientedBoundingBoxPointcloud; - this->nodeHandle.get_parameter("local_bounding_box/compute", - this->computeLocalBoundingBox); - this->computeLocalBoundingBox = this->computeLocalBoundingBox || - this->publishNoLocalBoundingBoxPointcloud; + this->nodeHandle.get_parameter("local_bounding_box/compute", this->computeLocalBoundingBox); + this->computeLocalBoundingBox = this->computeLocalBoundingBox || this->publishNoLocalBoundingBoxPointcloud; - this->nodeHandle.get_parameter("bounding_sphere/debug", - this->computeDebugBoundingSphere); + this->nodeHandle.get_parameter("bounding_sphere/debug", this->computeDebugBoundingSphere); - this->nodeHandle.get_parameter("bounding_box/debug", - this->computeDebugBoundingBox); + this->nodeHandle.get_parameter("bounding_box/debug", this->computeDebugBoundingBox); - this->nodeHandle.get_parameter("oriented_bounding_box/debug", - this->computeDebugOrientedBoundingBox); + this->nodeHandle.get_parameter("oriented_bounding_box/debug", this->computeDebugOrientedBoundingBox); - this->nodeHandle.get_parameter("local_bounding_box/debug", - this->computeDebugLocalBoundingBox); + this->nodeHandle.get_parameter("local_bounding_box/debug", this->computeDebugLocalBoundingBox); - this->nodeHandle.get_parameter("bounding_sphere/marker", - this->publishBoundingSphereMarker); + this->nodeHandle.get_parameter("bounding_sphere/marker", this->publishBoundingSphereMarker); - this->nodeHandle.get_parameter("bounding_box/marker", - this->publishBoundingBoxMarker); + this->nodeHandle.get_parameter("bounding_box/marker", this->publishBoundingBoxMarker); - this->nodeHandle.get_parameter("oriented_bounding_box/marker", - this->publishOrientedBoundingBoxMarker); + this->nodeHandle.get_parameter("oriented_bounding_box/marker", this->publishOrientedBoundingBoxMarker); - this->nodeHandle.get_parameter("local_bounding_box/marker", - this->publishLocalBoundingBoxMarker); + this->nodeHandle.get_parameter("local_bounding_box/marker", this->publishLocalBoundingBoxMarker); - this->nodeHandle.get_parameter("local_bounding_box/frame_id", - this->localBoundingBoxFrame); + this->nodeHandle.get_parameter("local_bounding_box/frame_id", this->localBoundingBoxFrame); - this->nodeHandle.get_parameter("debug/pcl/inside", - this->publishDebugPclInside); + this->nodeHandle.get_parameter("debug/pcl/inside", this->publishDebugPclInside); this->nodeHandle.get_parameter("debug/pcl/clip", this->publishDebugPclClip); - this->nodeHandle.get_parameter("debug/pcl/shadow", - this->publishDebugPclShadow); + this->nodeHandle.get_parameter("debug/pcl/shadow", this->publishDebugPclShadow); - this->nodeHandle.get_parameter("debug/marker/contains", - this->publishDebugContainsMarker); + this->nodeHandle.get_parameter("debug/marker/contains", this->publishDebugContainsMarker); - this->nodeHandle.get_parameter("debug/marker/shadow", - this->publishDebugShadowMarker); + this->nodeHandle.get_parameter("debug/marker/shadow", this->publishDebugShadowMarker); - this->nodeHandle.get_parameter("debug/marker/bounding_sphere", - this->publishDebugBsphereMarker); + this->nodeHandle.get_parameter("debug/marker/bounding_sphere", this->publishDebugBsphereMarker); - this->nodeHandle.get_parameter("debug/marker/bounding_box", - this->publishDebugBboxMarker); + this->nodeHandle.get_parameter("debug/marker/bounding_box", this->publishDebugBboxMarker); double tempInflationPadding; - this->nodeHandle.get_parameter("body_model/inflation/padding", - tempInflationPadding); + this->nodeHandle.get_parameter("body_model/inflation/padding", tempInflationPadding); const double inflationPadding = tempInflationPadding; double tempInflationScale; - this->nodeHandle.get_parameter("body_model/inflation/scale", - tempInflationScale); + this->nodeHandle.get_parameter("body_model/inflation/scale", tempInflationScale); const double inflationScale = tempInflationScale; - this->nodeHandle.get_parameter("body_model/inflation/contains_test/padding", - this->defaultContainsInflation.padding); + this->nodeHandle.get_parameter("body_model/inflation/contains_test/padding", this->defaultContainsInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/contains_test/scale", - this->defaultContainsInflation.scale); + this->nodeHandle.get_parameter("body_model/inflation/contains_test/scale", this->defaultContainsInflation.scale); - this->nodeHandle.get_parameter("body_model/inflation/shadow_test/padding", - this->defaultShadowInflation.padding); + this->nodeHandle.get_parameter("body_model/inflation/shadow_test/padding", this->defaultShadowInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/shadow_test/scale", - this->defaultShadowInflation.scale); + this->nodeHandle.get_parameter("body_model/inflation/shadow_test/scale", this->defaultShadowInflation.scale); - this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/padding", - this->defaultBsphereInflation.padding); + this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/padding", this->defaultBsphereInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/scale", - this->defaultBsphereInflation.scale); + this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/scale", this->defaultBsphereInflation.scale); - this->nodeHandle.get_parameter("body_model/inflation/bounding_box/padding", - this->defaultBboxInflation.padding); + this->nodeHandle.get_parameter("body_model/inflation/bounding_box/padding", this->defaultBboxInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/bounding_box/scale", - this->defaultBboxInflation.scale); + this->nodeHandle.get_parameter("body_model/inflation/bounding_box/scale", this->defaultBboxInflation.scale); // read per-link padding std::map perLinkInflationPadding; - this->nodeHandle.get_parameters("body_model/inflation/per_link/padding", - perLinkInflationPadding); + this->nodeHandle.get_parameters("body_model/inflation/per_link/padding", perLinkInflationPadding); - for (const auto &inflationPair : perLinkInflationPadding) { + for (const auto& inflationPair : perLinkInflationPadding) { bool containsOnly; bool shadowOnly; bool bsphereOnly; @@ -348,25 +280,24 @@ template bool RobotBodyFilter::configure() { linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); if (!shadowOnly && !bsphereOnly && !bboxOnly) - this->perLinkContainsInflation[linkName] = ScaleAndPadding( - this->defaultContainsInflation.scale, inflationPair.second); + this->perLinkContainsInflation[linkName] = + ScaleAndPadding(this->defaultContainsInflation.scale, inflationPair.second); if (!containsOnly && !bsphereOnly && !bboxOnly) - this->perLinkShadowInflation[linkName] = ScaleAndPadding( - this->defaultShadowInflation.scale, inflationPair.second); + this->perLinkShadowInflation[linkName] = + ScaleAndPadding(this->defaultShadowInflation.scale, inflationPair.second); if (!containsOnly && !shadowOnly && !bboxOnly) - this->perLinkBsphereInflation[linkName] = ScaleAndPadding( - this->defaultBsphereInflation.scale, inflationPair.second); + this->perLinkBsphereInflation[linkName] = + ScaleAndPadding(this->defaultBsphereInflation.scale, inflationPair.second); if (!containsOnly && !shadowOnly && !bsphereOnly) - this->perLinkBboxInflation[linkName] = ScaleAndPadding( - this->defaultBboxInflation.scale, inflationPair.second); + this->perLinkBboxInflation[linkName] = ScaleAndPadding(this->defaultBboxInflation.scale, inflationPair.second); } // read per-link scale std::map perLinkInflationScale; - this->nodeHandle.get_parameters("body_model/inflation/per_link/scale", - perLinkInflationScale); + this->nodeHandle.get_parameters("body_model/inflation/per_link/scale", perLinkInflationScale); - for (const auto &inflationPair : perLinkInflationScale) { + for (const auto& inflationPair : perLinkInflationScale) + { bool containsOnly; bool shadowOnly; bool bsphereOnly; @@ -378,38 +309,35 @@ template bool RobotBodyFilter::configure() { linkName = removeSuffix(linkName, BSPHERE_SUFFIX, &bsphereOnly); linkName = removeSuffix(linkName, BBOX_SUFFIX, &bboxOnly); - if (!shadowOnly && !bsphereOnly && !bboxOnly) { - if (this->perLinkContainsInflation.find(linkName) == - this->perLinkContainsInflation.end()) - this->perLinkContainsInflation[linkName] = ScaleAndPadding( - inflationPair.second, this->defaultContainsInflation.padding); + if (!shadowOnly && !bsphereOnly && !bboxOnly) + { + if (this->perLinkContainsInflation.find(linkName) == this->perLinkContainsInflation.end()) + this->perLinkContainsInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultContainsInflation.padding); else this->perLinkContainsInflation[linkName].scale = inflationPair.second; } - if (!containsOnly && !bsphereOnly && !bboxOnly) { - if (this->perLinkShadowInflation.find(linkName) == - this->perLinkShadowInflation.end()) - this->perLinkShadowInflation[linkName] = ScaleAndPadding( - inflationPair.second, this->defaultShadowInflation.padding); + if (!containsOnly && !bsphereOnly && !bboxOnly) + { + if (this->perLinkShadowInflation.find(linkName) == this->perLinkShadowInflation.end()) + this->perLinkShadowInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultShadowInflation.padding); else this->perLinkShadowInflation[linkName].scale = inflationPair.second; } - if (!containsOnly && !shadowOnly && !bboxOnly) { - if (this->perLinkBsphereInflation.find(linkName) == - this->perLinkBsphereInflation.end()) - this->perLinkBsphereInflation[linkName] = ScaleAndPadding( - inflationPair.second, this->defaultBsphereInflation.padding); + if (!containsOnly && !shadowOnly && !bboxOnly) + { + if (this->perLinkBsphereInflation.find(linkName) == this->perLinkBsphereInflation.end()) + this->perLinkBsphereInflation[linkName] = ScaleAndPadding(inflationPair.second, this->defaultBsphereInflation.padding); else this->perLinkBsphereInflation[linkName].scale = inflationPair.second; } - if (!containsOnly && !shadowOnly && !bsphereOnly) { - if (this->perLinkBboxInflation.find(linkName) == - this->perLinkBboxInflation.end()) - this->perLinkBboxInflation[linkName] = ScaleAndPadding( - inflationPair.second, this->defaultBboxInflation.padding); + if (!containsOnly && !shadowOnly && !bsphereOnly) + { + if (this->perLinkBboxInflation.find(linkName) == this->perLinkBboxInflation.end()) + this->perLinkBboxInflation[linkName] = + ScaleAndPadding(inflationPair.second, this->defaultBboxInflation.padding); else this->perLinkBboxInflation[linkName].scale = inflationPair.second; } @@ -420,45 +348,33 @@ template bool RobotBodyFilter::configure() { // Note: ROS2 does not by default have a set parameter, this was the // workaround std::vector tempLinksIgnoredInBoundingSphereVector; - this->nodeHandle.get_parameter("ignored_links/bounding_sphere", - tempLinksIgnoredInBoundingSphereVector); - this->linksIgnoredInBoundingSphere.insert( - tempLinksIgnoredInBoundingSphereVector.begin(), - tempLinksIgnoredInBoundingSphereVector.end()); + this->nodeHandle.get_parameter("ignored_links/bounding_sphere", tempLinksIgnoredInBoundingSphereVector); + this->linksIgnoredInBoundingSphere.insert(tempLinksIgnoredInBoundingSphereVector.begin(), + tempLinksIgnoredInBoundingSphereVector.end()); std::vector tempLinksIgnoredInBoundingBoxVector; - this->nodeHandle.get_parameter("ignored_links/bounding_box", - tempLinksIgnoredInBoundingBoxVector); - this->linksIgnoredInBoundingBox.insert( - tempLinksIgnoredInBoundingBoxVector.begin(), - tempLinksIgnoredInBoundingBoxVector.end()); + this->nodeHandle.get_parameter("ignored_links/bounding_box", tempLinksIgnoredInBoundingBoxVector); + this->linksIgnoredInBoundingBox.insert(tempLinksIgnoredInBoundingBoxVector.begin(), + tempLinksIgnoredInBoundingBoxVector.end()); std::vector tempLinksIgnoredInContainsTest; - this->nodeHandle.get_parameter("ignored_links/contains_test", - tempLinksIgnoredInContainsTest); - this->linksIgnoredInContainsTest.insert( - tempLinksIgnoredInContainsTest.begin(), - tempLinksIgnoredInContainsTest.end()); + this->nodeHandle.get_parameter("ignored_links/contains_test", tempLinksIgnoredInContainsTest); + this->linksIgnoredInContainsTest.insert(tempLinksIgnoredInContainsTest.begin(), tempLinksIgnoredInContainsTest.end()); std::vector tempLinksIgnoredInShadowTest; - this->nodeHandle.get_parameter("ignored_links/shadow_test", - tempLinksIgnoredInShadowTest); - this->linksIgnoredInShadowTest.insert(tempLinksIgnoredInShadowTest.begin(), - tempLinksIgnoredInShadowTest.end()); + this->nodeHandle.get_parameter("ignored_links/shadow_test", tempLinksIgnoredInShadowTest); + this->linksIgnoredInShadowTest.insert(tempLinksIgnoredInShadowTest.begin(), tempLinksIgnoredInShadowTest.end()); std::vector tempLinksIgnoredEverywhere; - this->nodeHandle.get_parameter("ignored_links/everywhere", - tempLinksIgnoredEverywhere); - this->linksIgnoredEverywhere.insert(tempLinksIgnoredEverywhere.begin(), - tempLinksIgnoredEverywhere.end()); + this->nodeHandle.get_parameter("ignored_links/everywhere", tempLinksIgnoredEverywhere); + this->linksIgnoredEverywhere.insert(tempLinksIgnoredEverywhere.begin(), tempLinksIgnoredEverywhere.end()); std::vector tempOnlyLinks; this->nodeHandle.get_parameter("only_links", tempOnlyLinks); this->onlyLinks.insert(tempOnlyLinks.begin(), tempOnlyLinks.end()); - this->nodeHandle.get_parameter( - "body_model/dynamic_robot_description/field_name", - this->robotDescriptionUpdatesFieldName); + this->nodeHandle.get_parameter("body_model/dynamic_robot_description/field_name", + this->robotDescriptionUpdatesFieldName); // // subscribe for robot_description param changes // this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( @@ -652,19 +568,14 @@ template bool RobotBodyFilter::configure() { // this->tfFramesWatchdog->start(); // } - { // initialize the robot body to be masked out + { // initialize the robot body to be masked out std::string robotUrdf; - while (!this->nodeHandle.get_parameter(this->robotDescriptionParam, - robotUrdf) || - robotUrdf.length() == 0) { + while (!this->nodeHandle.get_parameter(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { if (this->failWithoutRobotDescription) { - throw std::runtime_error( - "RobotBodyFilter: " + this->robotDescriptionParam + - " is empty or not set."); + throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); } - if (!rclcpp::ok()) - return false; + if (!rclcpp::ok()) return false; RCLCPP_ERROR(this->nodeHandle.get_logger(), "RobotBodyFilter: %s is empty or not set. Please, provide " @@ -676,24 +587,17 @@ template bool RobotBodyFilter::configure() { // happens when configure() is called again from update() (e.g. when // a new bag file started // playing) - if (!this->shapesToLinks.empty()) - this->clearRobotMask(); + if (!this->shapesToLinks.empty()) this->clearRobotMask(); this->addRobotMaskFromUrdf(robotUrdf); } - RCLCPP_INFO(nodeHandle.get_logger(), - "RobotBodyFilter: Successfullyconfigured."); - RCLCPP_INFO(nodeHandle.get_logger(), "Filtering data inframe %s", - this->filteringFrame.c_str()); - RCLCPP_INFO(nodeHandle.get_logger(), - "RobotBodyFilter: Filtering into the following categories:"); + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Successfullyconfigured."); + RCLCPP_INFO(nodeHandle.get_logger(), "Filtering data inframe %s", this->filteringFrame.c_str()); + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Filtering into the following categories:"); RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tOUTSIDE"); - if (doClipping) - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tCLIP"); - if (doContainsTest) - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tINSIDE"); - if (doShadowTest) - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tSHADOW"); + if (doClipping) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tCLIP"); + if (doContainsTest) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tINSIDE"); + if (doShadowTest) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tSHADOW"); // if (this->onlyLinks.empty()) { // if (this->linksIgnoredEverywhere.empty()) { @@ -757,14 +661,13 @@ bool RobotBodyFilterPointCloud2::configure() { template bool RobotBodyFilter::computeMask( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud, - std::vector &pointMask, - const std::string &sensorFrame) { - + const sensor_msgs::msg::PointCloud2& projectedPointCloud, + std::vector& pointMask, + const std::string& sensorFrame) { // this->modelMutex has to be already locked! const clock_t stopwatchOverall = clock(); - const auto &scanTime = projectedPointCloud.header.stamp; + const auto& scanTime = projectedPointCloud.header.stamp; // compute a mask of point indices for points from projectedPointCloud // that tells if they are inside or outside robot, or shadow points @@ -772,16 +675,14 @@ bool RobotBodyFilter::computeMask( if (!this->pointByPointScan) { Eigen::Vector3d sensorPosition; try { - const auto sensorTf = this->tfBuffer->lookupTransform( - this->filteringFrame, sensorFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout)); + const auto sensorTf = this->tfBuffer->lookupTransform(this->filteringFrame, sensorFrame, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout)); tf2::fromMsg(sensorTf.transform.translation, sensorPosition); - } catch (tf2::TransformException &e) { - RCLCPP_ERROR( - nodeHandle.get_logger(), - "RobotBodyFilter: Could not compute filtering mask due to this " - "TF exception: %s", - e.what()); + } catch (tf2::TransformException& e) { + RCLCPP_ERROR(nodeHandle.get_logger(), + "RobotBodyFilter: Could not compute filtering mask due to this " + "TF exception: %s", + e.what()); return false; } @@ -790,8 +691,7 @@ bool RobotBodyFilter::computeMask( // updates shapes according to tf cache (by calling getShapeTransform // for each shape) and masks contained points - this->shapeMask->maskContainmentAndShadows(projectedPointCloud, pointMask, - sensorPosition); + this->shapeMask->maskContainmentAndShadows(projectedPointCloud, pointMask, sensorPosition); } else { CloudConstIter x_it(projectedPointCloud, "x"); CloudConstIter y_it(projectedPointCloud, "y"); @@ -804,37 +704,31 @@ bool RobotBodyFilter::computeMask( pointMask.resize(num_points(projectedPointCloud)); double scanDuration = 0.0; - for (CloudConstIter stamps_end_it(projectedPointCloud, "stamps"); - stamps_end_it != stamps_end_it.end(); ++stamps_end_it) { - if ((*stamps_end_it) > static_cast(scanDuration)) - scanDuration = static_cast(*stamps_end_it); + for (CloudConstIter stamps_end_it(projectedPointCloud, "stamps"); stamps_end_it != stamps_end_it.end(); + ++stamps_end_it) { + if ((*stamps_end_it) > static_cast(scanDuration)) scanDuration = static_cast(*stamps_end_it); } - const rclcpp::Time afterScanTime( - rclcpp::Time(scanTime) + rclcpp::Duration::from_seconds(scanDuration)); + const rclcpp::Time afterScanTime(rclcpp::Time(scanTime) + rclcpp::Duration::from_seconds(scanDuration)); size_t updateBodyPosesEvery; - if (this->modelPoseUpdateInterval.seconds() == 0 && - this->modelPoseUpdateInterval.nanoseconds() == 0) { + if (this->modelPoseUpdateInterval.seconds() == 0 && this->modelPoseUpdateInterval.nanoseconds() == 0) { updateBodyPosesEvery = 1; } else { updateBodyPosesEvery = static_cast( - ceil(this->modelPoseUpdateInterval.seconds() / scanDuration * - num_points(projectedPointCloud))); + ceil(this->modelPoseUpdateInterval.seconds() / scanDuration * num_points(projectedPointCloud))); // prevent division by zero - if (updateBodyPosesEvery == 0) - updateBodyPosesEvery = 1; + if (updateBodyPosesEvery == 0) updateBodyPosesEvery = 1; } // prevent division by zero in ratio computation in case the pointcloud // isn't really taken point by point with different timestamps if (scanDuration == 0.0) { updateBodyPosesEvery = num_points(projectedPointCloud) + 1; - RCLCPP_WARN_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: sensor/point_by_point is set to true but " - "all points in the cloud have the same timestamp. You should" - " change the parameter to false to gain performance."); + RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: sensor/point_by_point is set to true but " + "all points in the cloud have the same timestamp. You should" + " change the parameter to false to gain performance."); } // update transforms cache, which is then used in body masking @@ -845,8 +739,8 @@ bool RobotBodyFilter::computeMask( RayCastingShapeMask::MaskValue mask; this->cacheLookupBetweenScansRatio = 0.0; - for (size_t i = 0; i < num_points(projectedPointCloud); ++i, ++x_it, ++y_it, - ++z_it, ++vp_x_it, ++vp_y_it, ++vp_z_it, ++stamps_it) { + for (size_t i = 0; i < num_points(projectedPointCloud); + ++i, ++x_it, ++y_it, ++z_it, ++vp_x_it, ++vp_y_it, ++vp_z_it, ++stamps_it) { point.x() = *x_it; point.y() = *y_it; point.z() = *z_it; @@ -859,19 +753,16 @@ bool RobotBodyFilter::computeMask( const auto updateBodyPoses = i % updateBodyPosesEvery == 0; if (updateBodyPoses && scanDuration > 0.0) - this->cacheLookupBetweenScansRatio = - static_cast(*stamps_it) / scanDuration; + this->cacheLookupBetweenScansRatio = static_cast(*stamps_it) / scanDuration; // updates shapes according to tf cache (by calling getShapeTransform // for each shape) and masks contained points - this->shapeMask->maskContainmentAndShadows(point, mask, viewPoint, - updateBodyPoses); + this->shapeMask->maskContainmentAndShadows(point, mask, viewPoint, updateBodyPoses); pointMask[i] = mask; } } - RCLCPP_DEBUG(nodeHandle.get_logger(), - "RobotBodyFilter: Mask computed in %.5f secs.", + RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); this->publishDebugPointClouds(projectedPointCloud, pointMask); @@ -881,39 +772,33 @@ bool RobotBodyFilter::computeMask( this->computeAndPublishOrientedBoundingBox(projectedPointCloud); this->computeAndPublishLocalBoundingBox(projectedPointCloud); - RCLCPP_DEBUG(nodeHandle.get_logger(), - "RobotBodyFilter: Filtering run time is %.5f secs.", + RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); return true; } -bool RobotBodyFilterLaserScan::update( - const sensor_msgs::msg::LaserScan &inputScan, - sensor_msgs::msg::LaserScan &filteredScan) { - const auto &headerScanTime = inputScan.header.stamp; - const auto &scanTime = rclcpp::Time(inputScan.header.stamp); +bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputScan, + sensor_msgs::msg::LaserScan& filteredScan) { + const auto& headerScanTime = inputScan.header.stamp; + const auto& scanTime = rclcpp::Time(inputScan.header.stamp); if (!this->configured_) { - RCLCPP_DEBUG( - nodeHandle.get_logger(), - "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " - "initialized.", - scanTime.seconds(), scanTime.nanoseconds()); + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < timeConfigured) && - ((scanTime + tfBufferLength) >= timeConfigured)) { - RCLCPP_DEBUG( - nodeHandle.get_logger(), - "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " - "initialized.", - scanTime.seconds(), scanTime.nanoseconds()); + if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) >= timeConfigured)) { + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < timeConfigured) && - ((scanTime + tfBufferLength) < timeConfigured)) { + if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) < timeConfigured)) { RCLCPP_WARN(nodeHandle.get_logger(), "RobotBodyFilter: Old TF data received. Clearing TF buffer and " "reconfiguring laser" @@ -930,13 +815,12 @@ bool RobotBodyFilterLaserScan::update( // Passing a sensorFrame does not make sense. Scan messages can't be // transformed to other frames. if (!this->sensorFrame.empty() && this->sensorFrame != scanFrame) { - RCLCPP_WARN_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: frames/sensor is set to frame_id '%s' different than " - "the frame_id of the incoming message '%s'. This is an invalid " - "configuration: " - "the frames/sensor parameter will be neglected.", - this->sensorFrame.c_str(), scanFrame.c_str()); + RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: frames/sensor is set to frame_id '%s' different than " + "the frame_id of the incoming message '%s'. This is an invalid " + "configuration: " + "the frames/sensor parameter will be neglected.", + this->sensorFrame.c_str(), scanFrame.c_str()); } if (!this->tfFramesWatchdog->isReachable(scanFrame)) { @@ -945,13 +829,11 @@ bool RobotBodyFilterLaserScan::update( "unreachable."); // if this->sensorFrame is empty, it can happen that we're not actually // monitoring the sensor frame, so start monitoring it - if (!this->tfFramesWatchdog->isMonitored(scanFrame)) - this->tfFramesWatchdog->addMonitoredFrame(scanFrame); + if (!this->tfFramesWatchdog->isMonitored(scanFrame)) this->tfFramesWatchdog->addMonitoredFrame(scanFrame); return false; } - if (this->requireAllFramesReachable && - !this->tfFramesWatchdog->areAllFramesReachable()) { + if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable()) { RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Throwing away scan since not all frames are " "reachable."); @@ -964,44 +846,35 @@ bool RobotBodyFilterLaserScan::update( filteredScan = inputScan; filteredScan.header.frame_id = scanFrame; filteredScan.range_min = fmax(inputScan.range_min, (float)this->minDistance); - if (this->maxDistance > 0.0) - filteredScan.range_max = - fmin(inputScan.range_max, (float)this->maxDistance); + if (this->maxDistance > 0.0) filteredScan.range_max = fmin(inputScan.range_max, (float)this->maxDistance); - { // acquire the lock here, because we work with the tfBuffer all the time + { // acquire the lock here, because we work with the tfBuffer all the time std::lock_guard guard(*this->modelMutex); - if (this->pointByPointScan) { // make sure we have all the tfs between - // sensor frame and fixedFrame during the time - // of scan acquisition - const auto scanDuration = - inputScan.ranges.size() * inputScan.time_increment; - const auto afterScanTime = - scanTime + rclcpp::Duration::from_seconds(scanDuration); + if (this->pointByPointScan) { // make sure we have all the tfs between + // sensor frame and fixedFrame during the time + // of scan acquisition + const auto scanDuration = inputScan.ranges.size() * inputScan.time_increment; + const auto afterScanTime = scanTime + rclcpp::Duration::from_seconds(scanDuration); std::string err; - if (!this->tfBuffer->canTransform( - this->fixedFrame, scanFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err) || - !this->tfBuffer->canTransform( - this->fixedFrame, scanFrame, afterScanTime, - remainingTime(afterScanTime, this->reachableTransformTimeout), - &err)) { + if (!this->tfBuffer->canTransform(this->fixedFrame, scanFrame, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err) || + !this->tfBuffer->canTransform(this->fixedFrame, scanFrame, afterScanTime, + remainingTime(afterScanTime, this->reachableTransformTimeout), &err)) { if (err.find("future") != std::string::npos) { const auto delay = nodeHandle.now() - scanTime; - auto &clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE( - nodeHandle.get_logger(), clk, 3, - "RobotBodyFilter: Cannot transform laser scan to " - "fixed frame. The scan is too much delayed (%s s). TF error: %s", - to_string(delay).c_str(), err.c_str()); + auto& clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform laser scan to " + "fixed frame. The scan is too much delayed (%s s). TF error: %s", + to_string(delay).c_str(), err.c_str()); } else { - auto &clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE( - nodeHandle.get_logger(), clk, 3, - "RobotBodyFilter: Cannot transform laser scan to " - "fixed frame. Something's wrong with TFs: %s", - err.c_str()); + auto& clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform laser scan to " + "fixed frame. Something's wrong with TFs: %s", + err.c_str()); } return false; } @@ -1010,50 +883,37 @@ bool RobotBodyFilterLaserScan::update( // The point cloud will have fields x, y, z, intensity (float32) and index // (int32) and for point-by-point scans also timestamp and viewpoint sensor_msgs::msg::PointCloud2 projectedPointCloud; - { // project the scan measurements to a point cloud in the filteringFrame + { // project the scan measurements to a point cloud in the filteringFrame sensor_msgs::msg::PointCloud2 tmpPointCloud; // the projected point cloud can omit some measurements if they are out of // the defined scan's range; for this case, the second channel ("index") // contains indices of the point cloud's points into the scan - auto channelOptions = laser_geometry::channel_option::Intensity | - laser_geometry::channel_option::Index; + auto channelOptions = laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index; if (this->pointByPointScan) { - RCLCPP_INFO_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: Applying complex laser scan projection."); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Applying complex laser scan projection."); // perform the complex laser scan projection - channelOptions |= laser_geometry::channel_option::Timestamp | - laser_geometry::channel_option::Viewpoint; + channelOptions |= laser_geometry::channel_option::Timestamp | laser_geometry::channel_option::Viewpoint; - laserProjector.transformLaserScanToPointCloud( - this->fixedFrame, inputScan, tmpPointCloud, *this->tfBuffer, -1, - channelOptions); + laserProjector.transformLaserScanToPointCloud(this->fixedFrame, inputScan, tmpPointCloud, *this->tfBuffer, -1, + channelOptions); } else { - RCLCPP_INFO_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: Applying simple laser scan projection."); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Applying simple laser scan projection."); // perform simple laser scan projection - laserProjector.projectLaser(inputScan, tmpPointCloud, -1.0, - channelOptions); + laserProjector.projectLaser(inputScan, tmpPointCloud, -1.0, channelOptions); } // convert to filtering frame if (tmpPointCloud.header.frame_id == this->filteringFrame) { projectedPointCloud = std::move(tmpPointCloud); } else { - RCLCPP_INFO_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: Transforming scan from frame %s to %s", - tmpPointCloud.header.frame_id.c_str(), - this->filteringFrame.c_str()); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Transforming scan from frame %s to %s", + tmpPointCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); std::string err; - if (!this->tfBuffer->canTransform( - this->filteringFrame, tmpPointCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), - &err)) { + if (!this->tfBuffer->canTransform(this->filteringFrame, tmpPointCloud.header.frame_id, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { // RCLCPP_ERROR_DELAYED_THROTTLE( // nodeHandle.get_logger(), 3, // "RobotBodyFilter: Cannot transform " @@ -1062,44 +922,39 @@ bool RobotBodyFilterLaserScan::update( return false; } - transformWithChannels(tmpPointCloud, projectedPointCloud, - *this->tfBuffer, this->filteringFrame, + transformWithChannels(tmpPointCloud, projectedPointCloud, *this->tfBuffer, this->filteringFrame, this->channelsToTransform); } } - RCLCPP_DEBUG(nodeHandle.get_logger(), - "RobotBodyFilter: Scan transformation run time is %.5f secs.", + RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Scan transformation run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); std::vector pointMask; - const auto success = - this->computeMask(projectedPointCloud, pointMask, scanFrame); + const auto success = this->computeMask(projectedPointCloud, pointMask, scanFrame); - if (!success) - return false; + if (!success) return false; - { // remove invalid points + { // remove invalid points const float INVALID_POINT_VALUE = std::numeric_limits::quiet_NaN(); try { - sensor_msgs::PointCloud2Iterator indexIt(projectedPointCloud, - "index"); + sensor_msgs::PointCloud2Iterator indexIt(projectedPointCloud, "index"); size_t indexInScan; for (const auto maskValue : pointMask) { switch (maskValue) { - case RayCastingShapeMask::MaskValue::INSIDE: - case RayCastingShapeMask::MaskValue::SHADOW: - case RayCastingShapeMask::MaskValue::CLIP: - indexInScan = static_cast(*indexIt); - filteredScan.ranges[indexInScan] = INVALID_POINT_VALUE; - break; - case RayCastingShapeMask::MaskValue::OUTSIDE: - break; + case RayCastingShapeMask::MaskValue::INSIDE: + case RayCastingShapeMask::MaskValue::SHADOW: + case RayCastingShapeMask::MaskValue::CLIP: + indexInScan = static_cast(*indexIt); + filteredScan.ranges[indexInScan] = INVALID_POINT_VALUE; + break; + case RayCastingShapeMask::MaskValue::OUTSIDE: + break; } ++indexIt; } - } catch (std::runtime_error &) { + } catch (std::runtime_error& ) { RCLCPP_ERROR(this->nodeHandle.get_logger(), "RobotBodyFilter: projectedPointCloud doesn't have field " "called 'index'," @@ -1112,46 +967,38 @@ bool RobotBodyFilterLaserScan::update( return true; } -bool RobotBodyFilterPointCloud2::update( - const sensor_msgs::msg::PointCloud2 &inputCloud, - sensor_msgs::msg::PointCloud2 &filteredCloud) { - const auto &headerScanTime = inputCloud.header.stamp; - const auto &scanTime = rclcpp::Time(headerScanTime); +bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inputCloud, + sensor_msgs::msg::PointCloud2& filteredCloud) { + const auto& headerScanTime = inputCloud.header.stamp; + const auto& scanTime = rclcpp::Time(headerScanTime); if (!this->configured_) { - RCLCPP_DEBUG( - nodeHandle.get_logger(), - "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " - "initialized.", - scanTime.seconds(), scanTime.nanoseconds()); + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < this->timeConfigured) && - ((scanTime + this->tfBufferLength) >= this->timeConfigured)) { - RCLCPP_DEBUG( - nodeHandle.get_logger(), - "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " - "initialized.", - scanTime.seconds(), scanTime.nanoseconds()); + if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) >= this->timeConfigured)) { + RCLCPP_DEBUG(nodeHandle.get_logger(), + "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " + "initialized.", + scanTime.seconds(), scanTime.nanoseconds()); return false; } - if ((scanTime < this->timeConfigured) && - ((scanTime + this->tfBufferLength) < this->timeConfigured)) { - RCLCPP_WARN( - nodeHandle.get_logger(), - "RobotBodyFilter: Old TF data received. Clearing TF buffer and " - "reconfiguring laser filter. If you're replaying a bag file, make " - "sure rosparam /use_sim_time is set to true"); + if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) < this->timeConfigured)) { + RCLCPP_WARN(nodeHandle.get_logger(), + "RobotBodyFilter: Old TF data received. Clearing TF buffer and " + "reconfiguring laser filter. If you're replaying a bag file, make " + "sure rosparam /use_sim_time is set to true"); this->configure(); return false; } const auto inputCloudFrame = - this->sensorFrame.empty() - ? stripLeadingSlash(inputCloud.header.frame_id, true) - : this->sensorFrame; + this->sensorFrame.empty() ? stripLeadingSlash(inputCloud.header.frame_id, true) : this->sensorFrame; if (!this->tfFramesWatchdog->isReachable(inputCloudFrame)) { RCLCPP_DEBUG(nodeHandle.get_logger(), @@ -1164,8 +1011,7 @@ bool RobotBodyFilterPointCloud2::update( return false; } - if (this->requireAllFramesReachable && - !this->tfFramesWatchdog->areAllFramesReachable()) { + if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable()) { RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Throwing away scan since not all frames are " "reachable."); @@ -1174,18 +1020,14 @@ bool RobotBodyFilterPointCloud2::update( bool hasStampsField = false; bool hasVpXField = false, hasVpYField = false, hasVpZField = false; - for (const auto &field : inputCloud.fields) { - if (field.name == "stamps" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) + for (const auto& field : inputCloud.fields) { + if (field.name == "stamps" && field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasStampsField = true; - else if (field.name == "vp_x" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) + else if (field.name == "vp_x" && field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasVpXField = true; - else if (field.name == "vp_y" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) + else if (field.name == "vp_y" && field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasVpYField = true; - else if (field.name == "vp_z" && - field.datatype == sensor_msgs::msg::PointField::FLOAT32) + else if (field.name == "vp_z" && field.datatype == sensor_msgs::msg::PointField::FLOAT32) hasVpZField = true; } @@ -1193,16 +1035,16 @@ bool RobotBodyFilterPointCloud2::update( if (this->pointByPointScan) { if (inputCloud.height != 1 && inputCloud.is_dense == 0) { - RCLCPP_WARN_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: The pointcloud seems to be an organized " - "pointcloud, which usually means it was captured all at once." - " Consider setting 'point_by_point_scan' to false to get a " - "more efficient computation."); + RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: The pointcloud seems to be an organized " + "pointcloud, which usually means it was captured all at once." + " Consider setting 'point_by_point_scan' to false to get a " + "more efficient computation."); } if (!hasStampsField || !hasVpXField || !hasVpYField || !hasVpZField) { - throw std::runtime_error("A point-by-point scan has to contain float32" - "fields 'stamps', 'vp_x', 'vp_y' and 'vp_z'."); + throw std::runtime_error( + "A point-by-point scan has to contain float32" + "fields 'stamps', 'vp_x', 'vp_y' and 'vp_z'."); } } else if (hasStampsField) { RCLCPP_WARN_ONCE(nodeHandle.get_logger(), @@ -1211,12 +1053,11 @@ bool RobotBodyFilterPointCloud2::update( "different time instant. Consider setting parameter " "'point_by_point_scan' to true to get correct results."); } else if (inputCloud.height == 1 && inputCloud.is_dense == 1) { - RCLCPP_WARN_ONCE( - nodeHandle.get_logger(), - "RobotBodyFilter: The pointcloud is dense, which usually means" - " it was captured each point at a different time instant. " - "Consider setting 'point_by_point_scan' to true to get a more" - " accurate version."); + RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + "RobotBodyFilter: The pointcloud is dense, which usually means" + " it was captured each point at a different time instant. " + "Consider setting 'point_by_point_scan' to true to get a more" + " accurate version."); } // Transform to filtering frame @@ -1225,26 +1066,22 @@ bool RobotBodyFilterPointCloud2::update( if (inputCloud.header.frame_id == this->filteringFrame) { transformedCloud = inputCloud; } else { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), - "RobotBodyFilter: Transforming cloud from frame %s to %s", - inputCloud.header.frame_id.c_str(), - this->filteringFrame.c_str()); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", + inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform( - this->filteringFrame, inputCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - auto &clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE( - nodeHandle.get_logger(), clk, 3, - "RobotBodyFilter: Cannot transform " - "point cloud to filtering frame. Something's wrong with TFs: %s", - err.c_str()); + if (!this->tfBuffer->canTransform(this->filteringFrame, inputCloud.header.frame_id, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + auto& clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform " + "point cloud to filtering frame. Something's wrong with TFs: %s", + err.c_str()); return false; } - transformWithChannels(inputCloud, transformedCloud, *this->tfBuffer, - this->filteringFrame, this->channelsToTransform); + transformWithChannels(inputCloud, transformedCloud, *this->tfBuffer, this->filteringFrame, + this->channelsToTransform); } // Compute the mask and use it (transform message only if sensorFrame is @@ -1253,53 +1090,44 @@ bool RobotBodyFilterPointCloud2::update( { std::lock_guard guard(*this->modelMutex); - const auto success = - this->computeMask(transformedCloud, pointMask, inputCloudFrame); - if (!success) - return false; + const auto success = this->computeMask(transformedCloud, pointMask, inputCloudFrame); + if (!success) return false; } // Filter the cloud sensor_msgs::msg::PointCloud2 tmpCloud; - CREATE_FILTERED_CLOUD( - transformedCloud, tmpCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::OUTSIDE)) + CREATE_FILTERED_CLOUD(transformedCloud, tmpCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::OUTSIDE)) // Transform to output frame if (tmpCloud.header.frame_id == this->outputFrame) { filteredCloud = std::move(tmpCloud); } else { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), - "RobotBodyFilter: Transforming cloud from frame %s to %s", - tmpCloud.header.frame_id.c_str(), - this->outputFrame.c_str()); + RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", + tmpCloud.header.frame_id.c_str(), this->outputFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform( - this->outputFrame, tmpCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - auto &clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE( - nodeHandle.get_logger(), clk, 3, - "RobotBodyFilter: Cannot transform " - "point cloud to output frame. Something's wrong with TFs: %s", - err.c_str()); + if (!this->tfBuffer->canTransform(this->outputFrame, tmpCloud.header.frame_id, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + auto& clk = *nodeHandle.get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform " + "point cloud to output frame. Something's wrong with TFs: %s", + err.c_str()); return false; } - transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, - this->outputFrame, this->channelsToTransform); + transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, this->outputFrame, this->channelsToTransform); } return true; } template -bool RobotBodyFilter::getShapeTransform( - point_containment_filter::ShapeHandle shapeHandle, - Eigen::Isometry3d &transform) const { +bool RobotBodyFilter::getShapeTransform(point_containment_filter::ShapeHandle shapeHandle, + Eigen::Isometry3d& transform) const { // make sure you locked this->modelMutex // check if the given shapeHandle has been registered to a link during @@ -1311,10 +1139,9 @@ bool RobotBodyFilter::getShapeTransform( return false; } - const auto &collision = this->shapesToLinks.at(shapeHandle); + const auto& collision = this->shapesToLinks.at(shapeHandle); - if (this->transformCache.find(collision.cacheKey) == - this->transformCache.end()) { + if (this->transformCache.find(collision.cacheKey) == this->transformCache.end()) { // do not log the error because shape mask would do it for us return false; } @@ -1322,20 +1149,18 @@ bool RobotBodyFilter::getShapeTransform( if (!this->pointByPointScan) { transform = *this->transformCache.at(collision.cacheKey); } else { - if (this->transformCacheAfterScan.find(collision.cacheKey) == - this->transformCacheAfterScan.end()) { + if (this->transformCacheAfterScan.find(collision.cacheKey) == this->transformCacheAfterScan.end()) { // do not log the error because shape mask would do it for us return false; } - const auto &tf1 = *this->transformCache.at(collision.cacheKey); - const auto &tf2 = *this->transformCacheAfterScan.at(collision.cacheKey); + const auto& tf1 = *this->transformCache.at(collision.cacheKey); + const auto& tf2 = *this->transformCacheAfterScan.at(collision.cacheKey); const Eigen::Quaterniond quat1(tf1.rotation().matrix()); const Eigen::Quaterniond quat2(tf1.rotation().matrix()); const auto r = this->cacheLookupBetweenScansRatio; - transform.translation() = - tf1.translation() * (1 - r) + tf2.translation() * r; + transform.translation() = tf1.translation() * (1 - r) + tf2.translation() * r; const Eigen::Quaterniond quat3 = quat1.slerp(r, quat2); transform.linear() = quat3.toRotationMatrix(); } @@ -1344,72 +1169,64 @@ bool RobotBodyFilter::getShapeTransform( } template -void RobotBodyFilter::updateTransformCache( - const rclcpp::Time &time, const rclcpp::Time &afterScanTime) { +void RobotBodyFilter::updateTransformCache(const rclcpp::Time& time, const rclcpp::Time& afterScanTime) { // make sure you locked this->modelMutex // clear the cache so that maskContainment always uses only these tf data and // not some older this->transformCache.clear(); - if (afterScanTime.seconds() != 0) - this->transformCacheAfterScan.clear(); + if (afterScanTime.seconds() != 0) this->transformCacheAfterScan.clear(); // iterate over all links corresponding to some masking shape and update their // cached transforms relative to fixed_frame - for (auto &shapeToLink : this->shapesToLinks) { - - const auto &collisionBody = shapeToLink.second; - const auto &collision = collisionBody.collision; - const auto &link = collisionBody.link; + for (auto& shapeToLink : this->shapesToLinks) { + const auto& collisionBody = shapeToLink.second; + const auto& collision = collisionBody.collision; + const auto& link = collisionBody.link; // here we assume the tf frames' names correspond to the link names const auto linkFrame = link->name; // the collision object may have a different origin than the visual, we need // to account for that - const auto &collisionOffsetTransform = - urdfPose2EigenTransform(collision->origin); + const auto& collisionOffsetTransform = urdfPose2EigenTransform(collision->origin); { auto linkTransformTfOptional = this->tfFramesWatchdog->lookupTransform( - linkFrame, time, - remainingTime(time, this->reachableTransformTimeout)); + linkFrame, time, remainingTime(time, this->reachableTransformTimeout)); - if (!linkTransformTfOptional) // has no value + if (!linkTransformTfOptional) // has no value continue; - const auto &linkTransformTf = linkTransformTfOptional.value(); - const auto &linkTransformEigen = tf2::transformToEigen(linkTransformTf); + const auto& linkTransformTf = linkTransformTfOptional.value(); + const auto& linkTransformEigen = tf2::transformToEigen(linkTransformTf); - const auto &transform = linkTransformEigen * collisionOffsetTransform; + const auto& transform = linkTransformEigen * collisionOffsetTransform; this->transformCache[collisionBody.cacheKey] = - std::allocate_shared( - Eigen::aligned_allocator(), transform); + std::allocate_shared(Eigen::aligned_allocator(), transform); } if (afterScanTime.seconds() != 0) { auto linkTransformTfOptional = this->tfFramesWatchdog->lookupTransform( - linkFrame, afterScanTime, - remainingTime(time, this->reachableTransformTimeout)); + linkFrame, afterScanTime, remainingTime(time, this->reachableTransformTimeout)); - if (!linkTransformTfOptional) // has no value + if (!linkTransformTfOptional) // has no value continue; - const auto &linkTransformTf = linkTransformTfOptional.value(); - const auto &linkTransformEigen = tf2::transformToEigen(linkTransformTf); + const auto& linkTransformTf = linkTransformTfOptional.value(); + const auto& linkTransformEigen = tf2::transformToEigen(linkTransformTf); - const auto &transform = linkTransformEigen * collisionOffsetTransform; + const auto& transform = linkTransformEigen * collisionOffsetTransform; this->transformCacheAfterScan[collisionBody.cacheKey] = - std::allocate_shared( - Eigen::aligned_allocator(), transform); + std::allocate_shared(Eigen::aligned_allocator(), transform); } } } template -void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { +void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { if (urdfModel.empty()) { RCLCPP_ERROR(nodeHandle.get_logger(), "RobotBodyFilter: Empty string passed as robot model to " @@ -1441,29 +1258,25 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { std::unordered_set ignoreInShadowTest; // add all model's collision links as masking shapes - for (const auto &links : parsedUrdfModel.links_) { - - const auto &link = links.second; + for (const auto& links : parsedUrdfModel.links_) { + const auto& link = links.second; // every link can have multiple collision elements size_t collisionIndex = 0; - for (const auto &collision : link->collision_array) { + for (const auto& collision : link->collision_array) { if (collision->geometry == nullptr) { - RCLCPP_WARN( - nodeHandle.get_logger(), - "RobotBodyFilter: Collision element without geometry found " - "in link %s of robot %s. " - "This collision element will not be filtered out.", - link->name.c_str(), parsedUrdfModel.getName().c_str()); - continue; // collisionIndex is intentionally not increased + RCLCPP_WARN(nodeHandle.get_logger(), + "RobotBodyFilter: Collision element without geometry found " + "in link %s of robot %s. " + "This collision element will not be filtered out.", + link->name.c_str(), parsedUrdfModel.getName().c_str()); + continue; // collisionIndex is intentionally not increased } const auto NAME_LINK = link->name; const auto NAME_COLLISION_NAME = "*::" + collision->name; - const auto NAME_LINK_COLLISION_NR = - link->name + "::" + std::to_string(collisionIndex); - const auto NAME_LINK_COLLISON_NAME = - link->name + "::" + collision->name; + const auto NAME_LINK_COLLISION_NR = link->name + "::" + std::to_string(collisionIndex); + const auto NAME_LINK_COLLISON_NAME = link->name + "::" + collision->name; const std::vector collisionNames = { NAME_LINK, @@ -1475,7 +1288,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { std::set collisionNamesSet; std::set collisionNamesContains; std::set collisionNamesShadow; - for (const auto &name : collisionNames) { + for (const auto& name : collisionNames) { collisionNamesSet.insert(name); collisionNamesContains.insert(name + CONTAINS_SUFFIX); collisionNamesShadow.insert(name + SHADOW_SUFFIX); @@ -1491,68 +1304,50 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { } // if the link is ignored, go on - if (!isSetIntersectionEmpty(collisionNamesSet, - this->linksIgnoredEverywhere)) { + if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredEverywhere)) { ++collisionIndex; continue; } const auto collisionShape = constructShape(*collision->geometry); - const auto shapeName = collision->name.empty() - ? NAME_LINK_COLLISION_NR - : NAME_LINK_COLLISON_NAME; + const auto shapeName = collision->name.empty() ? NAME_LINK_COLLISION_NR : NAME_LINK_COLLISON_NAME; // if the shape could not be constructed, ignore it (e.g. mesh was not // found) if (collisionShape == nullptr) { - RCLCPP_WARN( - nodeHandle.get_logger(), - "Could not construct shape for collision %s, ignoring it.", - shapeName.c_str()); + RCLCPP_WARN(nodeHandle.get_logger(), "Could not construct shape for collision %s, ignoring it.", + shapeName.c_str()); ++collisionIndex; continue; } // add the collision shape to shapeMask; the inflation parameters come // into play here - const auto containsTestInflation = - this->getLinkInflationForContainsTest(collisionNames); - const auto shadowTestInflation = - this->getLinkInflationForShadowTest(collisionNames); - const auto bsphereInflation = - this->getLinkInflationForBoundingSphere(collisionNames); - const auto bboxInflation = - this->getLinkInflationForBoundingBox(collisionNames); + const auto containsTestInflation = this->getLinkInflationForContainsTest(collisionNames); + const auto shadowTestInflation = this->getLinkInflationForShadowTest(collisionNames); + const auto bsphereInflation = this->getLinkInflationForBoundingSphere(collisionNames); + const auto bboxInflation = this->getLinkInflationForBoundingBox(collisionNames); const auto shapeHandle = this->shapeMask->addShape( - collisionShape, containsTestInflation.scale, - containsTestInflation.padding, shadowTestInflation.scale, - shadowTestInflation.padding, bsphereInflation.scale, - bsphereInflation.padding, bboxInflation.scale, + collisionShape, containsTestInflation.scale, containsTestInflation.padding, shadowTestInflation.scale, + shadowTestInflation.padding, bsphereInflation.scale, bsphereInflation.padding, bboxInflation.scale, bboxInflation.padding, false, shapeName); - this->shapesToLinks[shapeHandle.contains] = - this->shapesToLinks[shapeHandle.shadow] = - this->shapesToLinks[shapeHandle.bsphere] = - this->shapesToLinks[shapeHandle.bbox] = - CollisionBodyWithLink(collision, link, collisionIndex, - shapeHandle); - - if (!isSetIntersectionEmpty(collisionNamesSet, - this->linksIgnoredInBoundingSphere)) { + this->shapesToLinks[shapeHandle.contains] = this->shapesToLinks[shapeHandle.shadow] = + this->shapesToLinks[shapeHandle.bsphere] = this->shapesToLinks[shapeHandle.bbox] = + CollisionBodyWithLink(collision, link, collisionIndex, shapeHandle); + + if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInBoundingSphere)) { this->shapesIgnoredInBoundingSphere.insert(shapeHandle.bsphere); } - if (!isSetIntersectionEmpty(collisionNamesSet, - this->linksIgnoredInBoundingBox)) { + if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInBoundingBox)) { this->shapesIgnoredInBoundingBox.insert(shapeHandle.bbox); } - if (!isSetIntersectionEmpty(collisionNamesSet, - this->linksIgnoredInContainsTest)) { + if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInContainsTest)) { ignoreInContainsTest.insert(shapeHandle); } - if (!isSetIntersectionEmpty(collisionNamesSet, - this->linksIgnoredInShadowTest)) { + if (!isSetIntersectionEmpty(collisionNamesSet, this->linksIgnoredInShadowTest)) { ignoreInShadowTest.insert(shapeHandle); } @@ -1562,16 +1357,13 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { // no collision element found; only warn for links that are not ignored // and have at least one visual if (collisionIndex == 0 && !link->visual_array.empty()) { - if ((this->onlyLinks.empty() || - (this->onlyLinks.find(link->name) != this->onlyLinks.end())) && - this->linksIgnoredEverywhere.find(link->name) == - this->linksIgnoredEverywhere.end()) { - RCLCPP_WARN( - nodeHandle.get_logger(), - "RobotBodyFilter: No collision element found for link %s of " - "robot %s. This link will not be filtered out " - "from laser scans.", - link->name.c_str(), parsedUrdfModel.getName().c_str()); + if ((this->onlyLinks.empty() || (this->onlyLinks.find(link->name) != this->onlyLinks.end())) && + this->linksIgnoredEverywhere.find(link->name) == this->linksIgnoredEverywhere.end()) { + RCLCPP_WARN(nodeHandle.get_logger(), + "RobotBodyFilter: No collision element found for link %s of " + "robot %s. This link will not be filtered out " + "from laser scans.", + link->name.c_str(), parsedUrdfModel.getName().c_str()); } } } @@ -1582,23 +1374,22 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string &urdfModel) { this->shapeMask->updateInternalShapeLists(); std::set monitoredFrames; - for (const auto &shapeToLink : this->shapesToLinks) - monitoredFrames.insert(shapeToLink.second.link->name); + for (const auto& shapeToLink : this->shapesToLinks) monitoredFrames.insert(shapeToLink.second.link->name); // Issue #6: Monitor sensor frame even if it is not a part of the model - if (!this->sensorFrame.empty()) - monitoredFrames.insert(this->sensorFrame); + if (!this->sensorFrame.empty()) monitoredFrames.insert(this->sensorFrame); this->tfFramesWatchdog->setMonitoredFrames(monitoredFrames); } } -template void RobotBodyFilter::clearRobotMask() { +template +void RobotBodyFilter::clearRobotMask() { { std::lock_guard guard(*this->modelMutex); std::unordered_set removedMultiShapes; - for (const auto &shapeToLink : this->shapesToLinks) { - const auto &multiShape = shapeToLink.second.multiHandle; + for (const auto& shapeToLink : this->shapesToLinks) { + const auto& multiShape = shapeToLink.second.multiHandle; if (removedMultiShapes.find(multiShape) == removedMultiShapes.end()) { this->shapeMask->removeShape(multiShape, false); removedMultiShapes.insert(multiShape); @@ -1617,8 +1408,7 @@ template void RobotBodyFilter::clearRobotMask() { } template -void RobotBodyFilter::publishDebugMarkers( - const rclcpp::Time &scanTime) const { +void RobotBodyFilter::publishDebugMarkers(const rclcpp::Time& scanTime) const { // assume this->modelMutex is locked if (this->publishDebugContainsMarker) { @@ -1626,8 +1416,7 @@ void RobotBodyFilter::publishDebugMarkers( std_msgs::msg::ColorRGBA color; color.g = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForContainsTest(), - scanTime, color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForContainsTest(), scanTime, color, markerArray); this->debugContainsMarkerPublisher->publish(markerArray); } @@ -1636,8 +1425,7 @@ void RobotBodyFilter::publishDebugMarkers( std_msgs::msg::ColorRGBA color; color.b = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForShadowTest(), - scanTime, color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForShadowTest(), scanTime, color, markerArray); this->debugShadowMarkerPublisher->publish(markerArray); } @@ -1647,8 +1435,7 @@ void RobotBodyFilter::publishDebugMarkers( color.g = 1.0; color.b = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingSphere(), - scanTime, color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingSphere(), scanTime, color, markerArray); this->debugBsphereMarkerPublisher->publish(markerArray); } @@ -1658,46 +1445,40 @@ void RobotBodyFilter::publishDebugMarkers( color.r = 1.0; color.b = 1.0; color.a = 0.5; - createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingBox(), - scanTime, color, markerArray); + createBodyVisualizationMsg(this->shapeMask->getBodiesForBoundingBox(), scanTime, color, markerArray); this->debugBboxMarkerPublisher->publish(markerArray); } } template -void RobotBodyFilter::publishDebugPointClouds( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud, - const std::vector &pointMask) const { +void RobotBodyFilter::publishDebugPointClouds(const sensor_msgs::msg::PointCloud2& projectedPointCloud, + const std::vector& pointMask) const { if (this->publishDebugPclInside) { sensor_msgs::msg::PointCloud2 insideCloud; - CREATE_FILTERED_CLOUD( - projectedPointCloud, insideCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::INSIDE)); + CREATE_FILTERED_CLOUD(projectedPointCloud, insideCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::INSIDE)); this->debugPointCloudInsidePublisher->publish(insideCloud); } if (this->publishDebugPclClip) { sensor_msgs::msg::PointCloud2 clipCloud; - CREATE_FILTERED_CLOUD( - projectedPointCloud, clipCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::CLIP)); + CREATE_FILTERED_CLOUD(projectedPointCloud, clipCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::CLIP)); this->debugPointCloudClipPublisher->publish(clipCloud); } if (this->publishDebugPclShadow) { sensor_msgs::msg::PointCloud2 shadowCloud; - CREATE_FILTERED_CLOUD( - projectedPointCloud, shadowCloud, this->keepCloudsOrganized, - (pointMask[i] == RayCastingShapeMask::MaskValue::SHADOW)); + CREATE_FILTERED_CLOUD(projectedPointCloud, shadowCloud, this->keepCloudsOrganized, + (pointMask[i] == RayCastingShapeMask::MaskValue::SHADOW)); this->debugPointCloudShadowPublisher->publish(shadowCloud); } } template void RobotBodyFilter::computeAndPublishBoundingSphere( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { - if (!this->computeBoundingSphere && !this->computeDebugBoundingSphere) - return; + const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { + if (!this->computeBoundingSphere && !this->computeDebugBoundingSphere) return; // assume this->modelMutex is locked @@ -1709,18 +1490,15 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( this->shapeMask->updateBodyPoses(); } - const auto &scanTime = projectedPointCloud.header.stamp; + const auto& scanTime = projectedPointCloud.header.stamp; std::vector spheres; { visualization_msgs::msg::MarkerArray boundingSphereDebugMsg; - for (const auto &shapeHandleAndBody : - this->shapeMask->getBodiesForBoundingSphere()) { - const auto &shapeHandle = shapeHandleAndBody.first; - const auto &body = shapeHandleAndBody.second; + for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingSphere()) { + const auto& shapeHandle = shapeHandleAndBody.first; + const auto& body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingSphere.find(shapeHandle) != - this->shapesIgnoredInBoundingSphere.end()) - continue; + if (this->shapesIgnoredInBoundingSphere.find(shapeHandle) != this->shapesIgnoredInBoundingSphere.end()) continue; bodies::BoundingSphere sphere; body->computeBoundingSphere(sphere); @@ -1794,18 +1572,15 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( sensor_msgs::msg::PointCloud2 noSphereCloud; CREATE_FILTERED_CLOUD( projectedPointCloud, noSphereCloud, this->keepCloudsOrganized, - ((Eigen::Vector3d(*x_it, *y_it, *z_it) - boundingSphere.center) - .norm() > boundingSphere.radius)); + ((Eigen::Vector3d(*x_it, *y_it, *z_it) - boundingSphere.center).norm() > boundingSphere.radius)); this->scanPointCloudNoBoundingSpherePublisher->publish(noSphereCloud); } } } template -void RobotBodyFilter::computeAndPublishBoundingBox( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { - if (!this->computeBoundingBox && !this->computeDebugBoundingBox) - return; +void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { + if (!this->computeBoundingBox && !this->computeDebugBoundingBox) return; // assume this->modelMutex is locked @@ -1817,19 +1592,16 @@ void RobotBodyFilter::computeAndPublishBoundingBox( this->shapeMask->updateBodyPoses(); } - const auto &scanTime = projectedPointCloud.header.stamp; + const auto& scanTime = projectedPointCloud.header.stamp; std::vector boxes; { visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; - for (const auto &shapeHandleAndBody : - this->shapeMask->getBodiesForBoundingBox()) { - const auto &shapeHandle = shapeHandleAndBody.first; - const auto &body = shapeHandleAndBody.second; + for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { + const auto& shapeHandle = shapeHandleAndBody.first; + const auto& body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != - this->shapesIgnoredInBoundingBox.end()) - continue; + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; bodies::AxisAlignedBoundingBox box; body->computeBoundingBox(box); @@ -1900,7 +1672,6 @@ void RobotBodyFilter::computeAndPublishBoundingBox( // compute and publish the scan_point_cloud with robot bounding box removed if (this->publishNoBoundingBoxPointcloud) { - pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); @@ -1909,19 +1680,15 @@ void RobotBodyFilter::computeAndPublishBoundingBox( cropBox.setInputCloud(bboxCropInput); cropBox.setKeepOrganized(this->keepCloudsOrganized); - cropBox.setMin(Eigen::Vector4f(boxFloat.min()[0], boxFloat.min()[1], - boxFloat.min()[2], 0.0)); - cropBox.setMax(Eigen::Vector4f(boxFloat.max()[0], boxFloat.max()[1], - boxFloat.max()[2], 0.0)); + cropBox.setMin(Eigen::Vector4f(boxFloat.min()[0], boxFloat.min()[1], boxFloat.min()[2], 0.0)); + cropBox.setMax(Eigen::Vector4f(boxFloat.max()[0], boxFloat.max()[1], boxFloat.max()[2], 0.0)); pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud( - new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = - scanTime; // PCL strips precision of timestamp + boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp this->scanPointCloudNoBoundingBoxPublisher->publish(*boxFilteredCloud); } @@ -1930,10 +1697,8 @@ void RobotBodyFilter::computeAndPublishBoundingBox( template void RobotBodyFilter::computeAndPublishOrientedBoundingBox( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { - if (!this->computeOrientedBoundingBox && - !this->computeDebugOrientedBoundingBox) - return; + const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { + if (!this->computeOrientedBoundingBox && !this->computeDebugOrientedBoundingBox) return; // assume this->modelMutex is locked @@ -1945,19 +1710,16 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( this->shapeMask->updateBodyPoses(); } - const auto &scanTime = projectedPointCloud.header.stamp; + const auto& scanTime = projectedPointCloud.header.stamp; std::vector boxes; { visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; - for (const auto &shapeHandleAndBody : - this->shapeMask->getBodiesForBoundingBox()) { - const auto &shapeHandle = shapeHandleAndBody.first; - const auto &body = shapeHandleAndBody.second; + for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { + const auto& shapeHandle = shapeHandleAndBody.first; + const auto& body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != - this->shapesIgnoredInBoundingBox.end()) - continue; + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; bodies::OrientedBoundingBox box; // TODO: This is probabbly wrong, originally computeBoundingBox took in @@ -1973,10 +1735,8 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( msg.header.frame_id = this->filteringFrame; tf2::toMsg(box.getExtents(), msg.scale); - msg.pose.position = - tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); - msg.pose.orientation = - tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); + msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); msg.color.g = 1.0; msg.color.a = 0.5; @@ -1990,14 +1750,12 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( } if (this->computeDebugOrientedBoundingBox) { - this->orientedBoundingBoxDebugMarkerPublisher->publish( - boundingBoxDebugMsg); + this->orientedBoundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); } } if (this->computeOrientedBoundingBox) { - bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), - Eigen::Vector3d::Zero()); + bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); // TODO: fix this // bodies::mergeBoundingBoxesApprox(boxes, box); @@ -2020,10 +1778,8 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( msg.header.frame_id = this->filteringFrame; tf2::toMsg(box.getExtents(), msg.scale); - msg.pose.position = - tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); - msg.pose.orientation = - tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); + msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); msg.color.r = 1.0; msg.color.a = 0.5; @@ -2037,7 +1793,6 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( // compute and publish the scan_point_cloud with robot bounding box removed if (this->publishNoOrientedBoundingBoxPointcloud) { - pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); @@ -2050,68 +1805,54 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( cropBox.setMin(Eigen::Vector4f(-e.x() / 2, -e.y() / 2, -e.z() / 2, 0.0)); cropBox.setMax(Eigen::Vector4f(e.x() / 2, e.y() / 2, e.z() / 2, 0.0)); cropBox.setTranslation(box.getPose().translation().cast()); - cropBox.setRotation( - box.getPose().linear().eulerAngles(0, 1, 2).cast()); + cropBox.setRotation(box.getPose().linear().eulerAngles(0, 1, 2).cast()); pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud( - new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = - scanTime; // PCL strips precision of timestamp + boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - this->scanPointCloudNoOrientedBoundingBoxPublisher->publish( - *boxFilteredCloud); + this->scanPointCloudNoOrientedBoundingBoxPublisher->publish(*boxFilteredCloud); } } } template void RobotBodyFilter::computeAndPublishLocalBoundingBox( - const sensor_msgs::msg::PointCloud2 &projectedPointCloud) const { - if (!this->computeLocalBoundingBox && !this->computeDebugLocalBoundingBox) - return; + const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { + if (!this->computeLocalBoundingBox && !this->computeDebugLocalBoundingBox) return; // assume this->modelMutex is locked - const auto &scanTime = projectedPointCloud.header.stamp; + const auto& scanTime = projectedPointCloud.header.stamp; std::string err; try { - if (!this->tfBuffer->canTransform( - this->localBoundingBoxFrame, this->filteringFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - RCLCPP_ERROR(nodeHandle.get_logger(), - "Cannot get transform %s->%s. Error is %s.", - this->filteringFrame.c_str(), + if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, + remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + RCLCPP_ERROR(nodeHandle.get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), this->localBoundingBoxFrame.c_str(), err.c_str()); return; } - } catch (tf2::TransformException &e) { - RCLCPP_ERROR(nodeHandle.get_logger(), - "Cannot get transform %s->%s. Error is %s.", - this->filteringFrame.c_str(), + } catch (tf2::TransformException& e) { + RCLCPP_ERROR(nodeHandle.get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), this->localBoundingBoxFrame.c_str(), e.what()); return; } - const auto localTfMsg = this->tfBuffer->lookupTransform( - this->localBoundingBoxFrame, this->filteringFrame, scanTime); + const auto localTfMsg = this->tfBuffer->lookupTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime); const Eigen::Isometry3d localTf = tf2::transformToEigen(localTfMsg.transform); std::vector boxes; { visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; - for (const auto &shapeHandleAndBody : - this->shapeMask->getBodiesForBoundingBox()) { - const auto &shapeHandle = shapeHandleAndBody.first; - const auto &body = shapeHandleAndBody.second; + for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { + const auto& shapeHandle = shapeHandleAndBody.first; + const auto& body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != - this->shapesIgnoredInBoundingBox.end()) - continue; + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; bodies::AxisAlignedBoundingBox box; bodies::computeBoundingBoxAt(body, box, localTf * body->getPose()); @@ -2179,7 +1920,6 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( // compute and publish the scan_point_cloud with robot bounding box removed if (this->publishNoLocalBoundingBoxPointcloud) { - pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); @@ -2188,36 +1928,28 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( cropBox.setInputCloud(bboxCropInput); cropBox.setKeepOrganized(this->keepCloudsOrganized); - cropBox.setMin( - Eigen::Vector4f(box.min()[0], box.min()[1], box.min()[2], 0.0)); - cropBox.setMax( - Eigen::Vector4f(box.max()[0], box.max()[1], box.max()[2], 0.0)); + cropBox.setMin(Eigen::Vector4f(box.min()[0], box.min()[1], box.min()[2], 0.0)); + cropBox.setMax(Eigen::Vector4f(box.max()[0], box.max()[1], box.max()[2], 0.0)); const Eigen::Isometry3d localTfInv = localTf.inverse(); cropBox.setTranslation(localTfInv.translation().cast()); - cropBox.setRotation( - localTfInv.linear().eulerAngles(0, 1, 2).cast()); + cropBox.setRotation(localTfInv.linear().eulerAngles(0, 1, 2).cast()); pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud( - new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = - scanTime; // PCL strips precision of timestamp + boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - this->scanPointCloudNoLocalBoundingBoxPublisher->publish( - *boxFilteredCloud); + this->scanPointCloudNoLocalBoundingBoxPublisher->publish(*boxFilteredCloud); } } } template void RobotBodyFilter::createBodyVisualizationMsg( - const std::map - &bodies, - const rclcpp::Time &stamp, const std_msgs::msg::ColorRGBA &color, - visualization_msgs::msg::MarkerArray &markerArray) const { + const std::map& bodies, const rclcpp::Time& stamp, + const std_msgs::msg::ColorRGBA& color, visualization_msgs::msg::MarkerArray& markerArray) const { // when computing the markers for publication, we want to publish them to the // time of the scan, so we need to set cacheLookupBetweenScansRatio again to // zero @@ -2226,8 +1958,8 @@ void RobotBodyFilter::createBodyVisualizationMsg( this->shapeMask->updateBodyPoses(); } - for (const auto &shapeHandleAndBody : bodies) { - const auto &shapeHandle = shapeHandleAndBody.first; + for (const auto& shapeHandleAndBody : bodies) { + const auto& shapeHandle = shapeHandleAndBody.first; auto body = shapeHandleAndBody.second; visualization_msgs::msg::Marker msg; @@ -2285,9 +2017,9 @@ void RobotBodyFilter::createBodyVisualizationMsg( // } // template -// bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request &, +// bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request& , // std_srvs::srv::Trigger_Response -// &) { +// & ) { // std::string urdf; // auto success = this->nodeHandle.getParam(this->robotDescriptionParam, // urdf); @@ -2319,94 +2051,76 @@ void RobotBodyFilter::createBodyVisualizationMsg( // return true; // } -template RobotBodyFilter::~RobotBodyFilter() { - if (this->tfFramesWatchdog != nullptr) - this->tfFramesWatchdog->stop(); +template +RobotBodyFilter::~RobotBodyFilter() { + if (this->tfFramesWatchdog != nullptr) this->tfFramesWatchdog->stop(); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest( - const std::string &linkName) const { +ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest(const std::string& linkName) const { return this->getLinkInflationForContainsTest({linkName}); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest( - const std::vector &linkNames) const { - return this->getLinkInflation(linkNames, this->defaultContainsInflation, - this->perLinkContainsInflation); +ScaleAndPadding RobotBodyFilter::getLinkInflationForContainsTest(const std::vector& linkNames) const { + return this->getLinkInflation(linkNames, this->defaultContainsInflation, this->perLinkContainsInflation); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest( - const std::string &linkName) const { +ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest(const std::string& linkName) const { return this->getLinkInflationForShadowTest({linkName}); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest( - const std::vector &linkNames) const { - return this->getLinkInflation(linkNames, this->defaultShadowInflation, - this->perLinkShadowInflation); +ScaleAndPadding RobotBodyFilter::getLinkInflationForShadowTest(const std::vector& linkNames) const { + return this->getLinkInflation(linkNames, this->defaultShadowInflation, this->perLinkShadowInflation); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere( - const std::string &linkName) const { +ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere(const std::string& linkName) const { return this->getLinkInflationForBoundingSphere({linkName}); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere( - const std::vector &linkNames) const { - return this->getLinkInflation(linkNames, this->defaultBsphereInflation, - this->perLinkBsphereInflation); +ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingSphere(const std::vector& linkNames) const { + return this->getLinkInflation(linkNames, this->defaultBsphereInflation, this->perLinkBsphereInflation); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox( - const std::string &linkName) const { +ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox(const std::string& linkName) const { return this->getLinkInflationForBoundingBox({linkName}); } template -ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox( - const std::vector &linkNames) const { - return this->getLinkInflation(linkNames, this->defaultBboxInflation, - this->perLinkBboxInflation); +ScaleAndPadding RobotBodyFilter::getLinkInflationForBoundingBox(const std::vector& linkNames) const { + return this->getLinkInflation(linkNames, this->defaultBboxInflation, this->perLinkBboxInflation); } template ScaleAndPadding RobotBodyFilter::getLinkInflation( - const std::vector &linkNames, - const ScaleAndPadding &defaultInflation, - const std::map &perLinkInflation) const { + const std::vector& linkNames, const ScaleAndPadding& defaultInflation, + const std::map& perLinkInflation) const { ScaleAndPadding result = defaultInflation; - for (const auto &linkName : linkNames) { - if (perLinkInflation.find(linkName) != perLinkInflation.end()) - result = perLinkInflation.at(linkName); + for (const auto& linkName : linkNames) { + if (perLinkInflation.find(linkName) != perLinkInflation.end()) result = perLinkInflation.at(linkName); } return result; } -ScaleAndPadding::ScaleAndPadding(double scale, double padding) - : scale(scale), padding(padding) {} +ScaleAndPadding::ScaleAndPadding(double scale, double padding) : scale(scale), padding(padding) {} -bool ScaleAndPadding::operator==(const ScaleAndPadding &other) const { +bool ScaleAndPadding::operator==(const ScaleAndPadding& other) const { return this->scale == other.scale && this->padding == other.padding; } -bool ScaleAndPadding::operator!=(const ScaleAndPadding &other) const { - return !(*this == other); -} +bool ScaleAndPadding::operator!=(const ScaleAndPadding& other) const { return !(*this == other); } -} // namespace robot_body_filter +} // namespace robot_body_filter #include -PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, - filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterLaserScan, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(robot_body_filter::RobotBodyFilterPointCloud2, filters::FilterBase) diff --git a/src/utils/cloud.cpp b/src/utils/cloud.cpp index 51f0380..2490384 100644 --- a/src/utils/cloud.cpp +++ b/src/utils/cloud.cpp @@ -1,71 +1,66 @@ #include #define private protected #include -#include #undef private #include -namespace robot_body_filter { +namespace robot_body_filter +{ -size_t num_points(const Cloud &cloud) { +size_t num_points(const Cloud& cloud) +{ return size_t(cloud.height) * cloud.width; } -bool hasField(const Cloud &cloud, const std::string &fieldName) { - for (const auto &field : cloud.fields) { +bool hasField(const Cloud& cloud, const std::string& fieldName) { + for (const auto& field : cloud.fields) { if (field.name == fieldName) return true; } return false; } -sensor_msgs::PointField &getField(Cloud &cloud, const std::string &fieldName) { - for (auto &field : cloud.fields) { +sensor_msgs::PointField& getField(Cloud& cloud, const std::string& fieldName) { + for (auto& field : cloud.fields) { if (field.name == fieldName) return field; } - throw std::runtime_error(std::string("Field ") + fieldName + - " does not exist."); + throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); } -const sensor_msgs::PointField &getField(const Cloud &cloud, - const std::string &fieldName) { - for (const auto &field : cloud.fields) { +const sensor_msgs::PointField& getField(const Cloud& cloud, + const std::string& fieldName) { + for (const auto& field : cloud.fields) { if (field.name == fieldName) return field; } - throw std::runtime_error(std::string("Field ") + fieldName + - " does not exist."); + throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); } -size_t sizeOfPointField(const sensor_msgs::PointField &field) { +size_t sizeOfPointField(const sensor_msgs::PointField& field) +{ return sizeOfPointField(field.datatype); } -size_t sizeOfPointField(int datatype) { - if ((datatype == sensor_msgs::PointField::INT8) || - (datatype == sensor_msgs::PointField::UINT8)) +size_t sizeOfPointField(int datatype) +{ + if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8)) return 1u; - else if ((datatype == sensor_msgs::PointField::INT16) || - (datatype == sensor_msgs::PointField::UINT16)) + else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16)) return 2u; - else if ((datatype == sensor_msgs::PointField::INT32) || - (datatype == sensor_msgs::PointField::UINT32) || + else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) || (datatype == sensor_msgs::PointField::FLOAT32)) return 4u; else if (datatype == sensor_msgs::PointField::FLOAT64) return 8u; else - throw std::runtime_error(std::string("PointField of type ") + - std::to_string(datatype) + " does not exist"); + throw std::runtime_error(std::string("PointField of type ") + std::to_string(datatype) + " does not exist"); } -void copyChannelData(const Cloud &in, Cloud &out, - const std::string &fieldName) { +void copyChannelData(const Cloud& in, Cloud& out, const std::string& fieldName) { if (num_points(out) < num_points(in)) - throw std::runtime_error("Output cloud needs to be resized to fit the " - "number of points of the input cloud."); + throw std::runtime_error("Output cloud needs to be resized to fit the number of points of the input cloud."); GenericCloudConstIter dataIn(in, fieldName); GenericCloudIter dataOut(out, fieldName); @@ -73,44 +68,35 @@ void copyChannelData(const Cloud &in, Cloud &out, dataOut.copyData(dataIn); } + namespace impl { -template class V> -GenericCloudIteratorBase::GenericCloudIteratorBase( - C &cloudMsg, const std::string &fieldName) - : sensor_msgs::impl::PointCloud2IteratorBase(cloudMsg, - fieldName) { +template class V> +GenericCloudIteratorBase::GenericCloudIteratorBase(C& cloudMsg, const std::string& fieldName) + : sensor_msgs::impl::PointCloud2IteratorBase(cloudMsg, fieldName) { this->fieldSize = sizeOfPointField(getField(cloudMsg, fieldName)); } -template class V> -U *GenericCloudIteratorBase::getData() const { +template class V> +U* GenericCloudIteratorBase::getData() const { return this->data_; } -template -void GenericCloudIterator::copyData( - const GenericCloudConstIterator &otherIter) const { +template +void GenericCloudIterator::copyData(const GenericCloudConstIterator& otherIter) const { memcpy(this->getData(), otherIter.getData(), this->fieldSize); } -template -void GenericCloudIterator::copyData( - const GenericCloudIterator &otherIter) const { +template +void GenericCloudIterator::copyData(const GenericCloudIterator& otherIter) const { memcpy(this->getData(), otherIter.getData(), this->fieldSize); } // explicitly instantiate -template class GenericCloudIteratorBase; -template class GenericCloudIteratorBase< - unsigned char, const unsigned char, const unsigned char, - const sensor_msgs::PointCloud2, GenericCloudConstIterator>; +template class GenericCloudIteratorBase; +template class GenericCloudIteratorBase; template class GenericCloudIterator; template class GenericCloudConstIterator; -} // namespace impl +} -} // namespace robot_body_filter +} From 3387ef6e900e32d4f34d96afeeb3bb29b230848f Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 13:33:01 -0400 Subject: [PATCH 20/54] clang format --- .clang-format | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..c99f469 --- /dev/null +++ b/.clang-format @@ -0,0 +1,16 @@ +--- +BasedOnStyle: Google + +ColumnLimit: 120 +SortIncludes: Never +ReflowComments: false +ReferenceAlignment: Left +PointerAlignment: Left +FixNamespaceComments: false +BreakBeforeBraces: Custom +BraceWrapping: + AfterStruct: true +BreakConstructorInitializers: AfterColon +PackConstructorInitializers: BinPack +--- + From 7487e3ec05ef2b285f8b742206507181501d48dd Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 14:33:16 -0400 Subject: [PATCH 21/54] Robot Description Update ROS2 edition --- include/robot_body_filter/RobotBodyFilter.h | 8 +- src/RobotBodyFilter.cpp | 140 +++++++++----------- 2 files changed, 70 insertions(+), 78 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 1653b3e..22ffa3e 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -26,6 +26,7 @@ // #include #include #include +#include #include @@ -203,7 +204,7 @@ class RobotBodyFilter : public filters::FilterBase { std::string robotDescriptionParam; //! Subscriber for robot_description updates. - // ros::Subscriber robotDescriptionUpdatesListener; + rclcpp::Subscription::SharedPtr robotDescriptionUpdatesListener; //! Name of the field in the dynamic reconfigure message that contains robot model. std::string robotDescriptionUpdatesFieldName; @@ -402,10 +403,9 @@ class RobotBodyFilter : public filters::FilterBase { /** * \brief Callback handling update of the robot_description parameter using dynamic reconfigure. * - * \param newConfig The updated config. + * \param msg The new configuration. */ - // void robotDescriptionUpdated(dynamic_reconfigure::ConfigConstPtr - // newConfig); + void robotDescriptionUpdated(const std_msgs::msg::String::SharedPtr msg); /** * \brief Callback for ~reload_model service. Reloads the URDF from parameter. diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 9a51b0a..8d128e8 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -376,7 +376,18 @@ bool RobotBodyFilter::configure() { this->nodeHandle.get_parameter("body_model/dynamic_robot_description/field_name", this->robotDescriptionUpdatesFieldName); - // // subscribe for robot_description param changes + + + // subscribe for robot_description param changes + // NOTE: Should this be a service instead? + this->robotDescriptionUpdatesListener = this->nodeHandle.template create_subscription( + "dynamic_robot_model_server/parameter_updates", 10, + std::bind(&RobotBodyFilter::robotDescriptionUpdated, this, std::placeholders::_1)); + + // this->nodeHandle.subscribe( + // "dynamic_robot_model_server/parameter_updates", 10, + // &RobotBodyFilter::robotDescriptionUpdated, this); + // this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( // "dynamic_robot_model_server/parameter_updates", 10, // &RobotBodyFilter::robotDescriptionUpdated, this); @@ -1977,79 +1988,60 @@ void RobotBodyFilter::createBodyVisualizationMsg( } } -// TODO: update for ROS2 -// template -// void RobotBodyFilter::robotDescriptionUpdated( -// dynamic_reconfigure::ConfigConstPtr newConfig) { -// auto robotDescriptionIdx = static_cast(-1); -// for (size_t i = 0; i < newConfig->strs.size(); ++i) { -// if (newConfig->strs[i].name == this->robotDescriptionUpdatesFieldName) { -// robotDescriptionIdx = i; -// break; -// } -// } - -// // robot_description parameter was not found, so we don't have to restart -// the -// // filter -// if (robotDescriptionIdx == static_cast(-1)) -// return; - -// auto urdf = newConfig->strs[robotDescriptionIdx].value; - -// RCLCPP_INFO(nodeHandle.get_logger(), -// "RobotBodyFilter: Reloading robot model because of " -// "dynamic_reconfigure update. Filter operation stopped."); - -// this->tfFramesWatchdog->pause(); -// this->configured_ = false; - -// this->clearRobotMask(); -// this->addRobotMaskFromUrdf(urdf); - -// this->tfFramesWatchdog->unpause(); -// this->timeConfigured = nodeHandle.now(); -// this->configured_ = true; - -// RCLCPP_INFO( -// nodeHandle.get_logger(), -// "RobotBodyFilter: Robot model reloaded, resuming filter operation."); -// } - -// template -// bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request& , -// std_srvs::srv::Trigger_Response -// & ) { -// std::string urdf; -// auto success = this->nodeHandle.getParam(this->robotDescriptionParam, -// urdf); - -// if (!success) { -// ROS_ERROR_STREAM("RobotBodyFilter: Parameter " -// << this->robotDescriptionParam << " doesn't exist."); -// return false; -// } - -// RCLCPP_INFO( -// nodeHandle.get_logger(), -// "RobotBodyFilter: Reloading robot model because of trigger. Filter " -// "operation stopped."); - -// this->tfFramesWatchdog->pause(); -// this->configured_ = false; - -// this->clearRobotMask(); -// this->addRobotMaskFromUrdf(urdf); - -// this->tfFramesWatchdog->unpause(); -// this->timeConfigured = nodeHandle.now(); -// this->configured_ = true; - -// RCLCPP_INFO( -// nodeHandle.get_logger(), -// "RobotBodyFilter: Robot model reloaded, resuming filter operation."); -// return true; -// } +// ROS1 requires this dynamic reconfig, I believe we can simply get parameter again in ROS2 +template +void RobotBodyFilter::robotDescriptionUpdated(const std_msgs::msg::String::SharedPtr msg) { + const std::string newConfig = msg->data; + // robot_description parameter was not found, so we don't have to restart + // the filter + if (newConfig.empty()) return; + + auto urdf = newConfig; + + RCLCPP_INFO(nodeHandle.get_logger(), + "RobotBodyFilter: Reloading robot model because of " + "dynamic_reconfigure update. Filter operation stopped."); + + this->tfFramesWatchdog->pause(); + this->configured_ = false; + + this->clearRobotMask(); + this->addRobotMaskFromUrdf(urdf); + + this->tfFramesWatchdog->unpause(); + this->timeConfigured = nodeHandle.now(); + this->configured_ = true; + + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); +} + +template +bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request&, std_srvs::srv::Trigger_Response&) { + std::string urdf; + auto success = this->nodeHandle.getParam(this->robotDescriptionParam, urdf); + + if (!success) { + ROS_ERROR_STREAM("RobotBodyFilter: Parameter " << this->robotDescriptionParam << " doesn't exist."); + return false; + } + + RCLCPP_INFO(nodeHandle.get_logger(), + "RobotBodyFilter: Reloading robot model because of trigger. Filter " + "operation stopped."); + + this->tfFramesWatchdog->pause(); + this->configured_ = false; + + this->clearRobotMask(); + this->addRobotMaskFromUrdf(urdf); + + this->tfFramesWatchdog->unpause(); + this->timeConfigured = nodeHandle.now(); + this->configured_ = true; + + RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); + return true; +} template RobotBodyFilter::~RobotBodyFilter() { From 4fb943a6708f87a2acc9cd5a58f459e6a225b3fa Mon Sep 17 00:00:00 2001 From: Tyler Date: Thu, 18 Apr 2024 14:39:47 -0400 Subject: [PATCH 22/54] Update main.yml --- .github/workflows/main.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 4383ce5..5482982 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -11,8 +11,8 @@ jobs: strategy: matrix: env: - - {ROS_DISTRO: melodic, ROS_REPO: testing} - - {ROS_DISTRO: noetic, ROS_REPO: testing} + - {ROS_DISTRO: humble, ROS_REPO: testing} + - {ROS_DISTRO: rolling, ROS_REPO: testing} env: CCACHE_DIR: /github/home/.ccache # Directory for ccache (and how we enable ccache in industrial_ci) runs-on: ubuntu-latest From 64092d740ed1a917e68b9a5a98205cebc4d55809 Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 16:38:29 -0400 Subject: [PATCH 23/54] urdf model loading --- include/robot_body_filter/RobotBodyFilter.h | 2 +- src/RobotBodyFilter.cpp | 144 +++++++++----------- 2 files changed, 68 insertions(+), 78 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index 22ffa3e..b69f1f1 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -260,7 +260,7 @@ class RobotBodyFilter : public filters::FilterBase { rclcpp::Publisher::SharedPtr debugBboxMarkerPublisher; //! Service server for reloading robot model. - // ros::ServiceServer reloadRobotModelServiceServer; + rclcpp::Service::SharedPtr reloadRobotModelServiceServer; //! Whether to compute bounding sphere of the robot. bool computeBoundingSphere; diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 8d128e8..c085764 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -60,8 +60,8 @@ RobotBodyFilter::RobotBodyFilter() param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; this->nodeHandle.declare_parameter("minDistance", 0.0, param_desc); this->nodeHandle.declare_parameter("maxDistance", 0.0, param_desc); - param_desc.description = "body_model/robot_description_param"; - this->nodeHandle.declare_parameter("robot_description", "", param_desc); + param_desc.description = "body_model/robot_description_param name"; + this->nodeHandle.declare_parameter("body_model/robot_description_param", "", param_desc); this->nodeHandle.declare_parameter("filter/keep_clouds_organized", true); param_desc.description = "s"; this->nodeHandle.declare_parameter("filter/model_pose_update_interval", 0.0, param_desc); @@ -128,6 +128,10 @@ RobotBodyFilter::RobotBodyFilter() this->nodeHandle.declare_parameter("ignored_links/everywhere", ""); this->nodeHandle.declare_parameter("only_links", ""); this->nodeHandle.declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model"); + + this->nodeHandle.declare_parameter("frames/output", this->filteringFrame); + this->nodeHandle.declare_parameter("cloud/point_channels", std::vector{"vp_"}); + this->nodeHandle.declare_parameter("cloud/direction_channels", std::vector{"normal_"}); } template @@ -376,25 +380,19 @@ bool RobotBodyFilter::configure() { this->nodeHandle.get_parameter("body_model/dynamic_robot_description/field_name", this->robotDescriptionUpdatesFieldName); + this->nodeHandle.get_parameter("sensor/point_by_point", this->pointByPointScan); + // subscribe for robot_description param changes - // NOTE: Should this be a service instead? this->robotDescriptionUpdatesListener = this->nodeHandle.template create_subscription( "dynamic_robot_model_server/parameter_updates", 10, std::bind(&RobotBodyFilter::robotDescriptionUpdated, this, std::placeholders::_1)); - // this->nodeHandle.subscribe( - // "dynamic_robot_model_server/parameter_updates", 10, - // &RobotBodyFilter::robotDescriptionUpdated, this); + // this->reloadRobotModelServiceServer = this->nodeHandle.template create_service( + // "reload_model", std::bind(&RobotBodyFilter::triggerModelReload, this, std::placeholders::_1, std::placeholders::_2)); - // this->robotDescriptionUpdatesListener = this->nodeHandle.subscribe( - // "dynamic_robot_model_server/parameter_updates", 10, - // &RobotBodyFilter::robotDescriptionUpdated, this); - // this->reloadRobotModelServiceServer = - // this->privateNodeHandle.advertiseService( - // "reload_model", &RobotBodyFilter::triggerModelReload, this); // if (this->computeBoundingSphere) { // this->boundingSpherePublisher = @@ -554,30 +552,31 @@ bool RobotBodyFilter::configure() { // } // initialize the 3D body masking tool - // auto getShapeTransformCallback = - // std::bind(&RobotBodyFilter::getShapeTransform, this, - // std::placeholders::_1, std::placeholders::_2); shapeMask = - // std::make_unique(getShapeTransformCallback, - // this->minDistance, this->maxDistance, - // doClipping, doContainsTest, doShadowTest, maxShadowDistance); - - // // the other case happens when configure() is called again from + auto getShapeTransformCallback = + std::bind(&RobotBodyFilter::getShapeTransform, this, + std::placeholders::_1, std::placeholders::_2); shapeMask = + std::make_unique(getShapeTransformCallback, + this->minDistance, this->maxDistance, + doClipping, doContainsTest, doShadowTest, maxShadowDistance); + + // the other case happens when configure() is called again from // update() (e.g. when a new bag file - // // started playing) - // if (this->tfFramesWatchdog == nullptr) { - // std::set initialMonitoredFrames; - // if (!this->sensorFrame.empty()) - // { - // initialMonitoredFrames.insert(this->sensorFrame); - // } - - // this->tfFramesWatchdog = - // std::make_shared(this->filteringFrame, - // initialMonitoredFrames, this->tfBuffer, - // this->unreachableTransformTimeout, - // rclcpp::Rate(rclcpp::Duration(1.0))); - // this->tfFramesWatchdog->start(); - // } + // started playing) + if (this->tfFramesWatchdog == nullptr) { + std::set initialMonitoredFrames; + if (!this->sensorFrame.empty()) + { + initialMonitoredFrames.insert(this->sensorFrame); + } + // auto rateVar = rclcpp::Duration::from_seconds(1.0).nanoseconds(); + // auto loop_rate = rclcpp::Rate(rateVar); + // this->tfFramesWatchdog = + // std::make_shared(this->filteringFrame, + // initialMonitoredFrames, this->tfBuffer, + // this->unreachableTransformTimeout, + // loop_rate); + this->tfFramesWatchdog->start(); + } { // initialize the robot body to be masked out @@ -610,62 +609,53 @@ bool RobotBodyFilter::configure() { if (doContainsTest) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tINSIDE"); if (doShadowTest) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tSHADOW"); - // if (this->onlyLinks.empty()) { - // if (this->linksIgnoredEverywhere.empty()) { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering - // applied to all links."); - // } else { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering - // applied to all links except %s.", - // to_string(this->linksIgnoredEverywhere).c_str()); - // } - // } else { - // if (this->linksIgnoredEverywhere.empty()) { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering - // applied to links %s.", to_string(this->onlyLinks).c_str()); - // } else { - // RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering - // applied to links %s with these links excluded: %s.", - // to_string(this->onlyLinks).c_str(), - // to_string(this->linksIgnoredEverywhere).c_str()); - // } - // } + if (this->onlyLinks.empty()) { + if (this->linksIgnoredEverywhere.empty()) { + RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to all links."); + } else { + RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to all links except %s.", + to_string(this->linksIgnoredEverywhere).c_str()); + } + } else { + if (this->linksIgnoredEverywhere.empty()) { + RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); + } else { + RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", + to_string(this->onlyLinks).c_str(), + to_string(this->linksIgnoredEverywhere).c_str()); + } + } - // this->timeConfigured = rclcpp::Time::now(); + this->timeConfigured = this->nodeHandle.now(); return true; } bool RobotBodyFilterLaserScan::configure() { - // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", - // true); - - // bool success = RobotBodyFilter::configure(); + this->nodeHandle.declare_parameter("sensor/point_by_point", true); + this->pointByPointScan; + bool success = RobotBodyFilter::configure(); return false; } bool RobotBodyFilterPointCloud2::configure() { - // this->pointByPointScan = this->getParamVerbose("sensor/point_by_point", - // false); - - // bool success = RobotBodyFilter::configure(); - // if (!success) - // return false; + this->nodeHandle.declare_parameter("sensor/point_by_point", false); - // this->outputFrame = this->getParamVerbose("frames/output", - // this->filteringFrame); + bool success = RobotBodyFilter::configure(); + if (!success) return false; - // const auto pointChannels = this->getParamVerbose("cloud/point_channels", - // std::vector{"vp_"}); const auto directionChannels = - // this->getParamVerbose("cloud/direction_channels", - // std::vector{"normal_"}); + this->nodeHandle.get_parameter("frames/output", this->outputFrame); + std::vector tempPointChannels; + std::vector tempDirectionChannels; + this->nodeHandle.get_parameter("cloud/point_channels", tempPointChannels); + this->nodeHandle.get_parameter("cloud/direction_channels", tempDirectionChannels); + const auto pointChannels = tempPointChannels; + const auto directionChannels = tempDirectionChannels; - // for (const auto& channel : pointChannels) - // this->channelsToTransform[channel] = CloudChannelType::POINT; - // for (const auto& channel : directionChannels) - // this->channelsToTransform[channel] = CloudChannelType::DIRECTION; + for (const auto& channel : pointChannels) this->channelsToTransform[channel] = CloudChannelType::POINT; + for (const auto& channel : directionChannels) this->channelsToTransform[channel] = CloudChannelType::DIRECTION; - // stripLeadingSlash(this->outputFrame, true); + stripLeadingSlash(this->outputFrame, true); return true; } From b03850afbc594ec44e0116fe58c911ccde31fd23 Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 16:39:03 -0400 Subject: [PATCH 24/54] adding watchdog to cmake --- CMakeLists.txt | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index bf7b105..4b09946 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -64,6 +64,17 @@ include_directories( ${Dependency}_INCLUDE_DIRS ) +add_library(${PROJECT_NAME}_watchdog src/TFFramesWatchdog.cpp) +ament_target_dependencies(${PROJECT_NAME}_watchdog ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +target_include_directories(${PROJECT_NAME}_watchdog + PUBLIC + $ + $ + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + + add_library(${PROJECT_NAME}_filter src/RobotBodyFilter.cpp) ament_target_dependencies(${PROJECT_NAME}_filter ${THIS_PACKAGE_INCLUDE_DEPENDS}) From b20b07242b60084b782cbb9165c5fb723fc1b212 Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 17:56:49 -0400 Subject: [PATCH 25/54] tfwatchdogport --- include/robot_body_filter/TfFramesWatchdog.h | 8 ++-- src/TFFramesWatchdog.cpp | 49 +++++++++++--------- 2 files changed, 32 insertions(+), 25 deletions(-) diff --git a/include/robot_body_filter/TfFramesWatchdog.h b/include/robot_body_filter/TfFramesWatchdog.h index 6023800..cd2f387 100644 --- a/include/robot_body_filter/TfFramesWatchdog.h +++ b/include/robot_body_filter/TfFramesWatchdog.h @@ -25,11 +25,12 @@ namespace robot_body_filter { */ class TFFramesWatchdog { public: - TFFramesWatchdog(std::string robotFrame, + TFFramesWatchdog(rclcpp::Node::SharedPtr nodeHandle, + std::string robotFrame, std::set monitoredFrames, std::shared_ptr tfBuffer, rclcpp::Duration unreachableTfLookupTimeout = rclcpp::Duration(0, 100000000), // 0.1 sec - rclcpp::Rate unreachableFramesCheckRate = rclcpp::Rate(1.0)); + rclcpp::Rate::SharedPtr unreachableFramesCheckRate = std::make_shared(1.0)); virtual ~TFFramesWatchdog(); @@ -183,13 +184,14 @@ class TFFramesWatchdog { //! Timeout for canTransform() for figuring out if an unreachable frame became reachable. rclcpp::Duration unreachableTfLookupTimeout; //! Rate at which checking for unreachable frames will be done. - rclcpp::Rate unreachableFramesCheckRate; + rclcpp::Rate::SharedPtr unreachableFramesCheckRate; //! Lock this mutex any time you want to work with monitoredFrames or reachableFrames. mutable std::mutex framesMutex; private: std::thread thisThread; + rclcpp::Node::SharedPtr nodeHandle; //Node handle of robotBodyFilter needed for rclcpp logging and time }; } diff --git a/src/TFFramesWatchdog.cpp b/src/TFFramesWatchdog.cpp index bc84754..e4598fc 100644 --- a/src/TFFramesWatchdog.cpp +++ b/src/TFFramesWatchdog.cpp @@ -6,11 +6,14 @@ namespace robot_body_filter { -TFFramesWatchdog::TFFramesWatchdog(std::string robotFrame, +TFFramesWatchdog::TFFramesWatchdog( + rclcpp::Node::SharedPtr nodeHandle, + std::string robotFrame, std::set monitoredFrames, std::shared_ptr tfBuffer, - ros::Duration unreachableTfLookupTimeout, - ros::Rate unreachableFramesCheckRate): + rclcpp::Duration unreachableTfLookupTimeout, + rclcpp::Rate::SharedPtr unreachableFramesCheckRate): + nodeHandle(std::move(nodeHandle)), robotFrame(std::move(robotFrame)), monitoredFrames(std::move(monitoredFrames)), tfBuffer(tfBuffer), @@ -27,11 +30,11 @@ void TFFramesWatchdog::start() { void TFFramesWatchdog::run() { this->started = true; - while (!this->shouldStop && ros::ok()) { + while (!this->shouldStop && rclcpp::ok()) { if (!this->paused) { // the thread is paused when we want to change the stuff protected by framesMutex. this->searchForReachableFrames(); } - this->unreachableFramesCheckRate.sleep(); + this->unreachableFramesCheckRate->sleep(); } } @@ -42,7 +45,7 @@ bool TFFramesWatchdog::isRunning() const { void TFFramesWatchdog::searchForReachableFrames() { - const ros::Time time = ros::Time::now(); + const rclcpp::Time time = nodeHandle->now(); // detect all unreachable frames // we can't join this loop with the following one, because we need to lock the framesMutex, but @@ -66,12 +69,13 @@ void TFFramesWatchdog::searchForReachableFrames() std::string err; if (this->tfBuffer->canTransform(this->robotFrame, frame, time, this->unreachableTfLookupTimeout, &err)) { this->markReachable(frame); - ROS_DEBUG("TFFramesWatchdog (%s): Frame %s became reachable at %i.%i", - this->robotFrame.c_str(), frame.c_str(), time.sec, time.nsec); + RCLCPP_DEBUG(this->nodeHandle->get_logger(),"TFFramesWatchdog (%s): Frame %s became reachable at %i.%i", + this->robotFrame.c_str(), frame.c_str(), time.seconds(), time.nanoseconds()); } else { - ROS_WARN_DELAYED_THROTTLE(3, - "TFFramesWatchdog (%s): Frame %s is not reachable! Cause: %s", - this->robotFrame.c_str(), frame.c_str(), err.c_str()); + auto &clk = *nodeHandle->get_clock(); + RCLCPP_WARN_THROTTLE(this->nodeHandle->get_logger(), clk, 3, + "TFFramesWatchdog (%s): Frame %s is not reachable! Cause: %s", this->robotFrame.c_str(), + frame.c_str(), err.c_str()); } } } @@ -85,13 +89,13 @@ void TFFramesWatchdog::unpause() { } void TFFramesWatchdog::stop() { - ROS_INFO("Stopping TF watchdog."); + RCLCPP_INFO(this->nodeHandle->get_logger(), "Stopping TF watchdog."); this->shouldStop = true; this->paused = true; if (this->started && this->thisThread.joinable()) this->thisThread.join(); // segfaults without this line - ROS_INFO("TF watchdog stopped."); + RCLCPP_INFO(this->nodeHandle->get_logger(), "TF watchdog stopped."); } void TFFramesWatchdog::clear() { @@ -101,10 +105,10 @@ void TFFramesWatchdog::clear() { reachableFrames.clear(); } -optional TFFramesWatchdog::lookupTransform( +optional TFFramesWatchdog::lookupTransform( const std::string &frame, - const ros::Time &time, - const ros::Duration &timeout, + const rclcpp::Time &time, + const rclcpp::Duration &timeout, std::string *errstr) { if (!this->started) @@ -114,7 +118,7 @@ optional TFFramesWatchdog::lookupTransform( std::lock_guard guard(this->framesMutex); if (!this->isMonitoredNoLock(frame)) { - ROS_WARN("TFFramesWatchdog (%s): Frame %s is not yet monitored, starting " + RCLCPP_WARN(this->nodeHandle->get_logger(), "TFFramesWatchdog (%s): Frame %s is not yet monitored, starting " "monitoring it.", this->robotFrame.c_str(), frame.c_str()); this->addMonitoredFrameNoLock(frame); // this lookup is lost, same as if the frame is unreachable @@ -133,10 +137,10 @@ optional TFFramesWatchdog::lookupTransform( if (!this->tfBuffer->canTransform(this->robotFrame, frame, time, remainingTime(time, timeout), errstr)) { - ROS_WARN_THROTTLE(3, - "TFFramesWatchdog (%s): Frame %s became unreachable. Cause: %s", - this->robotFrame.c_str(), frame.c_str(), errstr->c_str()); - + auto &clk = *nodeHandle->get_clock(); + RCLCPP_WARN_THROTTLE(this->nodeHandle->get_logger(), clk, 3, + "TFFramesWatchdog (%s): Frame %s became unreachable. Cause: %s", this->robotFrame.c_str(), + frame.c_str(), errstr->c_str()); // if we couldn't get TF for this reachable frame, mark it unreachable this->markUnreachable(frame); return nullopt; @@ -147,7 +151,8 @@ optional TFFramesWatchdog::lookupTransform( return this->tfBuffer->lookupTransform( this->robotFrame, frame, time, remainingTime(time, timeout)); } catch (tf2::LookupException&) { - ROS_WARN_DELAYED_THROTTLE(3, + auto &clk = *nodeHandle->get_clock(); + RCLCPP_WARN_THROTTLE(this->nodeHandle->get_logger(), clk, 3, "TFFramesWatchdog (%s): Frame %s is not reachable. Cause: %s", this->robotFrame.c_str(), frame.c_str(), errstr->c_str()); From ba403ef23127eacf6ed0b52771002004ac1d0415 Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 18:14:12 -0400 Subject: [PATCH 26/54] Switch to shared ptr for better portability between threads --- include/robot_body_filter/RobotBodyFilter.h | 2 +- src/RobotBodyFilter.cpp | 443 ++++++++++---------- src/TFFramesWatchdog.cpp | 2 +- 3 files changed, 222 insertions(+), 225 deletions(-) diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index b69f1f1..c6a326e 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -111,7 +111,7 @@ class RobotBodyFilter : public filters::FilterBase { protected: //! Handle of the node this filter runs in. - rclcpp::Node nodeHandle; + rclcpp::Node::SharedPtr nodeHandle; rclcpp::Node privateNodeHandle; /** \brief If true, suppose that every point in the scan was captured at a diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index c085764..e485ed2 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -40,7 +40,7 @@ namespace robot_body_filter { template RobotBodyFilter::RobotBodyFilter() : privateNodeHandle("~"), - nodeHandle("robot_body_filter"), + nodeHandle(std::make_shared("robot_body_filter")), modelPoseUpdateInterval(0, 0), reachableTransformTimeout(0, 0), unreachableTransformTimeout(0, 0), @@ -51,87 +51,87 @@ RobotBodyFilter::RobotBodyFilter() auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; param_desc.description = "frames/fixed"; - this->nodeHandle.declare_parameter("fixedFrame", "base_link", param_desc); + this->nodeHandle->declare_parameter("fixedFrame", "base_link", param_desc); param_desc.description = "frames/sensor"; - this->nodeHandle.declare_parameter("sensorFrame", "", param_desc); + this->nodeHandle->declare_parameter("sensorFrame", "", param_desc); param_desc.description = "frames/filtering"; - this->nodeHandle.declare_parameter("filteringFrame", this->fixedFrame, param_desc); + this->nodeHandle->declare_parameter("filteringFrame", this->fixedFrame, param_desc); param_desc.description = "m"; param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; - this->nodeHandle.declare_parameter("minDistance", 0.0, param_desc); - this->nodeHandle.declare_parameter("maxDistance", 0.0, param_desc); + this->nodeHandle->declare_parameter("minDistance", 0.0, param_desc); + this->nodeHandle->declare_parameter("maxDistance", 0.0, param_desc); param_desc.description = "body_model/robot_description_param name"; - this->nodeHandle.declare_parameter("body_model/robot_description_param", "", param_desc); - this->nodeHandle.declare_parameter("filter/keep_clouds_organized", true); + this->nodeHandle->declare_parameter("body_model/robot_description_param", "", param_desc); + this->nodeHandle->declare_parameter("filter/keep_clouds_organized", true); param_desc.description = "s"; - this->nodeHandle.declare_parameter("filter/model_pose_update_interval", 0.0, param_desc); - this->nodeHandle.declare_parameter("filter/do_clipping", true); - this->nodeHandle.declare_parameter("filter/do_contains_test", true); - this->nodeHandle.declare_parameter("filter/do_shadow_test", true); + this->nodeHandle->declare_parameter("filter/model_pose_update_interval", 0.0, param_desc); + this->nodeHandle->declare_parameter("filter/do_clipping", true); + this->nodeHandle->declare_parameter("filter/do_contains_test", true); + this->nodeHandle->declare_parameter("filter/do_shadow_test", true); param_desc.description = "m"; - this->nodeHandle.declare_parameter("filter/max_shadow_distance", this->maxDistance, param_desc); + this->nodeHandle->declare_parameter("filter/max_shadow_distance", this->maxDistance, param_desc); param_desc.description = "s"; - this->nodeHandle.declare_parameter("transforms/timeout/reachable", 0.1, param_desc); - this->nodeHandle.declare_parameter("transforms/timeout/unreachable", 0.2, param_desc); - this->nodeHandle.declare_parameter("transforms/require_all_reachable", false); - this->nodeHandle.declare_parameter("bounding_sphere/publish_cut_out_pointcloud", false); - this->nodeHandle.declare_parameter("bounding_box/publish_cut_out_pointcloud", false); - this->nodeHandle.declare_parameter("oriented_bounding_box/publish_cut_out_pointcloud", false); - this->nodeHandle.declare_parameter("local_bounding_box/publish_cut_out_pointcloud", false); - this->nodeHandle.declare_parameter("bounding_sphere/compute", false); - this->nodeHandle.declare_parameter("bounding_box/compute", false); - this->nodeHandle.declare_parameter("oriented_bounding_box/compute", false); - this->nodeHandle.declare_parameter("local_bounding_box/compute", false); - this->nodeHandle.declare_parameter("bounding_sphere/debug", false); - this->nodeHandle.declare_parameter("bounding_box/debug", false); - this->nodeHandle.declare_parameter("oriented_bounding_box/debug", false); - this->nodeHandle.declare_parameter("local_bounding_box/debug", false); - this->nodeHandle.declare_parameter("bounding_sphere/marker", false); - this->nodeHandle.declare_parameter("bounding_box/marker", false); - this->nodeHandle.declare_parameter("oriented_bounding_box/marker", false); - this->nodeHandle.declare_parameter("local_bounding_box/marker", false); - this->nodeHandle.declare_parameter("local_bounding_box/frame_id", this->fixedFrame); - this->nodeHandle.declare_parameter("debug/pcl/inside", false); - this->nodeHandle.declare_parameter("debug/pcl/clip", false); - this->nodeHandle.declare_parameter("debug/pcl/shadow", false); - this->nodeHandle.declare_parameter("debug/marker/contains", false); - this->nodeHandle.declare_parameter("debug/marker/shadow", false); - this->nodeHandle.declare_parameter("debug/marker/bounding_sphere", false); - this->nodeHandle.declare_parameter("debug/marker/bounding_box", false); + this->nodeHandle->declare_parameter("transforms/timeout/reachable", 0.1, param_desc); + this->nodeHandle->declare_parameter("transforms/timeout/unreachable", 0.2, param_desc); + this->nodeHandle->declare_parameter("transforms/require_all_reachable", false); + this->nodeHandle->declare_parameter("bounding_sphere/publish_cut_out_pointcloud", false); + this->nodeHandle->declare_parameter("bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle->declare_parameter("oriented_bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle->declare_parameter("local_bounding_box/publish_cut_out_pointcloud", false); + this->nodeHandle->declare_parameter("bounding_sphere/compute", false); + this->nodeHandle->declare_parameter("bounding_box/compute", false); + this->nodeHandle->declare_parameter("oriented_bounding_box/compute", false); + this->nodeHandle->declare_parameter("local_bounding_box/compute", false); + this->nodeHandle->declare_parameter("bounding_sphere/debug", false); + this->nodeHandle->declare_parameter("bounding_box/debug", false); + this->nodeHandle->declare_parameter("oriented_bounding_box/debug", false); + this->nodeHandle->declare_parameter("local_bounding_box/debug", false); + this->nodeHandle->declare_parameter("bounding_sphere/marker", false); + this->nodeHandle->declare_parameter("bounding_box/marker", false); + this->nodeHandle->declare_parameter("oriented_bounding_box/marker", false); + this->nodeHandle->declare_parameter("local_bounding_box/marker", false); + this->nodeHandle->declare_parameter("local_bounding_box/frame_id", this->fixedFrame); + this->nodeHandle->declare_parameter("debug/pcl/inside", false); + this->nodeHandle->declare_parameter("debug/pcl/clip", false); + this->nodeHandle->declare_parameter("debug/pcl/shadow", false); + this->nodeHandle->declare_parameter("debug/marker/contains", false); + this->nodeHandle->declare_parameter("debug/marker/shadow", false); + this->nodeHandle->declare_parameter("debug/marker/bounding_sphere", false); + this->nodeHandle->declare_parameter("debug/marker/bounding_box", false); param_desc.description = "m"; - this->nodeHandle.declare_parameter("body_model/inflation/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/padding", 0.0, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/scale", 1.0); // NOTE: Default changed from inflationPadding/inflationScale to 0.0/1.0 - this->nodeHandle.declare_parameter("body_model/inflation/contains_test/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/contains_test/scale", 1.0); - this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/shadow_test/scale", 1.0); - this->nodeHandle.declare_parameter("body_model/inflation/bounding_sphere/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/bounding_sphere/scale", 1.0); - this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/padding", 0.0, param_desc); - this->nodeHandle.declare_parameter("body_model/inflation/bounding_box/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/contains_test/padding", 0.0, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/contains_test/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/padding", 0.0, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_sphere/padding", 0.0, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_sphere/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_box/padding", 0.0, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_box/scale", 1.0); // TODO: This initialization may be incorrect, not sure if I understand how // this works // https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html#ae5ab12777100f65bd09163814dbbf486 // this might need to be initialized with the names of each link - this->nodeHandle.declare_parameters("body_model/inflation/per_link/padding", std::map()); + this->nodeHandle->declare_parameters("body_model/inflation/per_link/padding", std::map()); - this->nodeHandle.declare_parameters("body_model/inflation/per_link/scale", std::map()); + this->nodeHandle->declare_parameters("body_model/inflation/per_link/scale", std::map()); // Note: some of these are init to empty strings, originally they are vectors // of strings - this->nodeHandle.declare_parameter("ignored_links/bounding_sphere", ""); - this->nodeHandle.declare_parameter("ignored_links/bounding_box", ""); - this->nodeHandle.declare_parameter("ignored_links/contains_test", ""); - this->nodeHandle.declare_parameter("ignored_links/shadow_test", "laser"); - this->nodeHandle.declare_parameter("ignored_links/everywhere", ""); - this->nodeHandle.declare_parameter("only_links", ""); - this->nodeHandle.declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model"); - - this->nodeHandle.declare_parameter("frames/output", this->filteringFrame); - this->nodeHandle.declare_parameter("cloud/point_channels", std::vector{"vp_"}); - this->nodeHandle.declare_parameter("cloud/direction_channels", std::vector{"normal_"}); + this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", ""); + this->nodeHandle->declare_parameter("ignored_links/bounding_box", ""); + this->nodeHandle->declare_parameter("ignored_links/contains_test", ""); + this->nodeHandle->declare_parameter("ignored_links/shadow_test", "laser"); + this->nodeHandle->declare_parameter("ignored_links/everywhere", ""); + this->nodeHandle->declare_parameter("only_links", ""); + this->nodeHandle->declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model"); + + this->nodeHandle->declare_parameter("frames/output", this->filteringFrame); + this->nodeHandle->declare_parameter("cloud/point_channels", std::vector{"vp_"}); + this->nodeHandle->declare_parameter("cloud/direction_channels", std::vector{"normal_"}); } template @@ -140,7 +140,7 @@ bool RobotBodyFilter::configure() { // rclcpp::Duration(60.0), "s"); if (this->tfBuffer == nullptr) { tf2::Duration tf2_duration = tf2_ros::fromRclcpp(this->tfBufferLength); - this->tfBuffer = std::make_shared(nodeHandle.get_clock(), tf2_duration); + this->tfBuffer = std::make_shared(nodeHandle->get_clock(), tf2_duration); this->tfListener = std::make_unique(*this->tfBuffer); } else { // clear the TF buffer (useful if calling configure() after receiving old TF @@ -148,128 +148,128 @@ bool RobotBodyFilter::configure() { this->tfBuffer->clear(); } - this->nodeHandle.get_parameter("fixedFrame", this->fixedFrame); + this->nodeHandle->get_parameter("fixedFrame", this->fixedFrame); stripLeadingSlash(this->fixedFrame, true); - this->nodeHandle.get_parameter("sensorFrame", this->sensorFrame); + this->nodeHandle->get_parameter("sensorFrame", this->sensorFrame); stripLeadingSlash(this->sensorFrame, true); - this->nodeHandle.get_parameter("filteringFrame", this->filteringFrame); + this->nodeHandle->get_parameter("filteringFrame", this->filteringFrame); stripLeadingSlash(this->sensorFrame, true); - this->nodeHandle.get_parameter("minDistance", this->minDistance); - this->nodeHandle.get_parameter("maxDistance", this->maxDistance); - this->nodeHandle.get_parameter("body_model/robot_description_param", this->robotDescriptionParam); - this->nodeHandle.get_parameter("filter/keep_clouds_organized", this->keepCloudsOrganized); + this->nodeHandle->get_parameter("minDistance", this->minDistance); + this->nodeHandle->get_parameter("maxDistance", this->maxDistance); + this->nodeHandle->get_parameter("body_model/robot_description_param", this->robotDescriptionParam); + this->nodeHandle->get_parameter("filter/keep_clouds_organized", this->keepCloudsOrganized); double tempModelPoseUpdateInterval; - this->nodeHandle.get_parameter("filter/model_pose_update_interval", tempModelPoseUpdateInterval); + this->nodeHandle->get_parameter("filter/model_pose_update_interval", tempModelPoseUpdateInterval); this->modelPoseUpdateInterval = rclcpp::Duration::from_seconds(tempModelPoseUpdateInterval); bool tempDoClipping; - this->nodeHandle.get_parameter("filter/do_clipping", tempDoClipping); + this->nodeHandle->get_parameter("filter/do_clipping", tempDoClipping); const bool doClipping = tempDoClipping; bool tempDoContainsTest; - this->nodeHandle.get_parameter("filter/do_contains_test", tempDoContainsTest); + this->nodeHandle->get_parameter("filter/do_contains_test", tempDoContainsTest); const bool doContainsTest = tempDoContainsTest; bool tempDoShadowTest; - this->nodeHandle.get_parameter("filter/do_shadow_test", tempDoShadowTest); + this->nodeHandle->get_parameter("filter/do_shadow_test", tempDoShadowTest); const bool doShadowTest = tempDoShadowTest; double tempMaxShadowDistance; - this->nodeHandle.get_parameter("filter/max_shadow_distance", tempMaxShadowDistance); + this->nodeHandle->get_parameter("filter/max_shadow_distance", tempMaxShadowDistance); const double maxShadowDistance = tempMaxShadowDistance; double tempReachableTransformTimeout; - this->nodeHandle.get_parameter("transforms/timeout/reachable", tempReachableTransformTimeout); + this->nodeHandle->get_parameter("transforms/timeout/reachable", tempReachableTransformTimeout); this->reachableTransformTimeout = rclcpp::Duration::from_seconds(tempReachableTransformTimeout); double tempUnreachableTransformTimeout; - this->nodeHandle.get_parameter("transforms/timeout/unreachable", tempUnreachableTransformTimeout); + this->nodeHandle->get_parameter("transforms/timeout/unreachable", tempUnreachableTransformTimeout); this->unreachableTransformTimeout = rclcpp::Duration::from_seconds(tempUnreachableTransformTimeout); - this->nodeHandle.get_parameter("transforms/require_all_reachable", this->requireAllFramesReachable); + this->nodeHandle->get_parameter("transforms/require_all_reachable", this->requireAllFramesReachable); - this->nodeHandle.get_parameter("bounding_sphere/publish_cut_out_pointcloud", this->publishNoBoundingSpherePointcloud); + this->nodeHandle->get_parameter("bounding_sphere/publish_cut_out_pointcloud", this->publishNoBoundingSpherePointcloud); - this->nodeHandle.get_parameter("bounding_box/publish_cut_out_pointcloud", this->publishNoBoundingBoxPointcloud); + this->nodeHandle->get_parameter("bounding_box/publish_cut_out_pointcloud", this->publishNoBoundingBoxPointcloud); - this->nodeHandle.get_parameter("oriented_bounding_box/publish_cut_out_pointcloud", + this->nodeHandle->get_parameter("oriented_bounding_box/publish_cut_out_pointcloud", this->publishNoOrientedBoundingBoxPointcloud); - this->nodeHandle.get_parameter("local_bounding_box/publish_cut_out_pointcloud", + this->nodeHandle->get_parameter("local_bounding_box/publish_cut_out_pointcloud", this->publishNoLocalBoundingBoxPointcloud); - this->nodeHandle.get_parameter("bounding_sphere/compute", this->computeBoundingSphere); + this->nodeHandle->get_parameter("bounding_sphere/compute", this->computeBoundingSphere); this->computeBoundingSphere = this->computeBoundingSphere || this->publishNoBoundingSpherePointcloud; - this->nodeHandle.get_parameter("bounding_box/compute", this->computeBoundingBox); + this->nodeHandle->get_parameter("bounding_box/compute", this->computeBoundingBox); this->computeBoundingBox = this->computeBoundingBox || this->publishNoBoundingBoxPointcloud; - this->nodeHandle.get_parameter("oriented_bounding_box/compute", this->computeOrientedBoundingBox); + this->nodeHandle->get_parameter("oriented_bounding_box/compute", this->computeOrientedBoundingBox); this->computeOrientedBoundingBox = this->computeOrientedBoundingBox || this->publishNoOrientedBoundingBoxPointcloud; - this->nodeHandle.get_parameter("local_bounding_box/compute", this->computeLocalBoundingBox); + this->nodeHandle->get_parameter("local_bounding_box/compute", this->computeLocalBoundingBox); this->computeLocalBoundingBox = this->computeLocalBoundingBox || this->publishNoLocalBoundingBoxPointcloud; - this->nodeHandle.get_parameter("bounding_sphere/debug", this->computeDebugBoundingSphere); + this->nodeHandle->get_parameter("bounding_sphere/debug", this->computeDebugBoundingSphere); - this->nodeHandle.get_parameter("bounding_box/debug", this->computeDebugBoundingBox); + this->nodeHandle->get_parameter("bounding_box/debug", this->computeDebugBoundingBox); - this->nodeHandle.get_parameter("oriented_bounding_box/debug", this->computeDebugOrientedBoundingBox); + this->nodeHandle->get_parameter("oriented_bounding_box/debug", this->computeDebugOrientedBoundingBox); - this->nodeHandle.get_parameter("local_bounding_box/debug", this->computeDebugLocalBoundingBox); + this->nodeHandle->get_parameter("local_bounding_box/debug", this->computeDebugLocalBoundingBox); - this->nodeHandle.get_parameter("bounding_sphere/marker", this->publishBoundingSphereMarker); + this->nodeHandle->get_parameter("bounding_sphere/marker", this->publishBoundingSphereMarker); - this->nodeHandle.get_parameter("bounding_box/marker", this->publishBoundingBoxMarker); + this->nodeHandle->get_parameter("bounding_box/marker", this->publishBoundingBoxMarker); - this->nodeHandle.get_parameter("oriented_bounding_box/marker", this->publishOrientedBoundingBoxMarker); + this->nodeHandle->get_parameter("oriented_bounding_box/marker", this->publishOrientedBoundingBoxMarker); - this->nodeHandle.get_parameter("local_bounding_box/marker", this->publishLocalBoundingBoxMarker); + this->nodeHandle->get_parameter("local_bounding_box/marker", this->publishLocalBoundingBoxMarker); - this->nodeHandle.get_parameter("local_bounding_box/frame_id", this->localBoundingBoxFrame); + this->nodeHandle->get_parameter("local_bounding_box/frame_id", this->localBoundingBoxFrame); - this->nodeHandle.get_parameter("debug/pcl/inside", this->publishDebugPclInside); + this->nodeHandle->get_parameter("debug/pcl/inside", this->publishDebugPclInside); - this->nodeHandle.get_parameter("debug/pcl/clip", this->publishDebugPclClip); + this->nodeHandle->get_parameter("debug/pcl/clip", this->publishDebugPclClip); - this->nodeHandle.get_parameter("debug/pcl/shadow", this->publishDebugPclShadow); + this->nodeHandle->get_parameter("debug/pcl/shadow", this->publishDebugPclShadow); - this->nodeHandle.get_parameter("debug/marker/contains", this->publishDebugContainsMarker); + this->nodeHandle->get_parameter("debug/marker/contains", this->publishDebugContainsMarker); - this->nodeHandle.get_parameter("debug/marker/shadow", this->publishDebugShadowMarker); + this->nodeHandle->get_parameter("debug/marker/shadow", this->publishDebugShadowMarker); - this->nodeHandle.get_parameter("debug/marker/bounding_sphere", this->publishDebugBsphereMarker); + this->nodeHandle->get_parameter("debug/marker/bounding_sphere", this->publishDebugBsphereMarker); - this->nodeHandle.get_parameter("debug/marker/bounding_box", this->publishDebugBboxMarker); + this->nodeHandle->get_parameter("debug/marker/bounding_box", this->publishDebugBboxMarker); double tempInflationPadding; - this->nodeHandle.get_parameter("body_model/inflation/padding", tempInflationPadding); + this->nodeHandle->get_parameter("body_model/inflation/padding", tempInflationPadding); const double inflationPadding = tempInflationPadding; double tempInflationScale; - this->nodeHandle.get_parameter("body_model/inflation/scale", tempInflationScale); + this->nodeHandle->get_parameter("body_model/inflation/scale", tempInflationScale); const double inflationScale = tempInflationScale; - this->nodeHandle.get_parameter("body_model/inflation/contains_test/padding", this->defaultContainsInflation.padding); + this->nodeHandle->get_parameter("body_model/inflation/contains_test/padding", this->defaultContainsInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/contains_test/scale", this->defaultContainsInflation.scale); + this->nodeHandle->get_parameter("body_model/inflation/contains_test/scale", this->defaultContainsInflation.scale); - this->nodeHandle.get_parameter("body_model/inflation/shadow_test/padding", this->defaultShadowInflation.padding); + this->nodeHandle->get_parameter("body_model/inflation/shadow_test/padding", this->defaultShadowInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/shadow_test/scale", this->defaultShadowInflation.scale); + this->nodeHandle->get_parameter("body_model/inflation/shadow_test/scale", this->defaultShadowInflation.scale); - this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/padding", this->defaultBsphereInflation.padding); + this->nodeHandle->get_parameter("body_model/inflation/bounding_sphere/padding", this->defaultBsphereInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/bounding_sphere/scale", this->defaultBsphereInflation.scale); + this->nodeHandle->get_parameter("body_model/inflation/bounding_sphere/scale", this->defaultBsphereInflation.scale); - this->nodeHandle.get_parameter("body_model/inflation/bounding_box/padding", this->defaultBboxInflation.padding); + this->nodeHandle->get_parameter("body_model/inflation/bounding_box/padding", this->defaultBboxInflation.padding); - this->nodeHandle.get_parameter("body_model/inflation/bounding_box/scale", this->defaultBboxInflation.scale); + this->nodeHandle->get_parameter("body_model/inflation/bounding_box/scale", this->defaultBboxInflation.scale); // read per-link padding std::map perLinkInflationPadding; - this->nodeHandle.get_parameters("body_model/inflation/per_link/padding", perLinkInflationPadding); + this->nodeHandle->get_parameters("body_model/inflation/per_link/padding", perLinkInflationPadding); for (const auto& inflationPair : perLinkInflationPadding) { bool containsOnly; @@ -298,7 +298,7 @@ bool RobotBodyFilter::configure() { // read per-link scale std::map perLinkInflationScale; - this->nodeHandle.get_parameters("body_model/inflation/per_link/scale", perLinkInflationScale); + this->nodeHandle->get_parameters("body_model/inflation/per_link/scale", perLinkInflationScale); for (const auto& inflationPair : perLinkInflationScale) { @@ -352,94 +352,94 @@ bool RobotBodyFilter::configure() { // Note: ROS2 does not by default have a set parameter, this was the // workaround std::vector tempLinksIgnoredInBoundingSphereVector; - this->nodeHandle.get_parameter("ignored_links/bounding_sphere", tempLinksIgnoredInBoundingSphereVector); + this->nodeHandle->get_parameter("ignored_links/bounding_sphere", tempLinksIgnoredInBoundingSphereVector); this->linksIgnoredInBoundingSphere.insert(tempLinksIgnoredInBoundingSphereVector.begin(), tempLinksIgnoredInBoundingSphereVector.end()); std::vector tempLinksIgnoredInBoundingBoxVector; - this->nodeHandle.get_parameter("ignored_links/bounding_box", tempLinksIgnoredInBoundingBoxVector); + this->nodeHandle->get_parameter("ignored_links/bounding_box", tempLinksIgnoredInBoundingBoxVector); this->linksIgnoredInBoundingBox.insert(tempLinksIgnoredInBoundingBoxVector.begin(), tempLinksIgnoredInBoundingBoxVector.end()); std::vector tempLinksIgnoredInContainsTest; - this->nodeHandle.get_parameter("ignored_links/contains_test", tempLinksIgnoredInContainsTest); + this->nodeHandle->get_parameter("ignored_links/contains_test", tempLinksIgnoredInContainsTest); this->linksIgnoredInContainsTest.insert(tempLinksIgnoredInContainsTest.begin(), tempLinksIgnoredInContainsTest.end()); std::vector tempLinksIgnoredInShadowTest; - this->nodeHandle.get_parameter("ignored_links/shadow_test", tempLinksIgnoredInShadowTest); + this->nodeHandle->get_parameter("ignored_links/shadow_test", tempLinksIgnoredInShadowTest); this->linksIgnoredInShadowTest.insert(tempLinksIgnoredInShadowTest.begin(), tempLinksIgnoredInShadowTest.end()); std::vector tempLinksIgnoredEverywhere; - this->nodeHandle.get_parameter("ignored_links/everywhere", tempLinksIgnoredEverywhere); + this->nodeHandle->get_parameter("ignored_links/everywhere", tempLinksIgnoredEverywhere); this->linksIgnoredEverywhere.insert(tempLinksIgnoredEverywhere.begin(), tempLinksIgnoredEverywhere.end()); std::vector tempOnlyLinks; - this->nodeHandle.get_parameter("only_links", tempOnlyLinks); + this->nodeHandle->get_parameter("only_links", tempOnlyLinks); this->onlyLinks.insert(tempOnlyLinks.begin(), tempOnlyLinks.end()); - this->nodeHandle.get_parameter("body_model/dynamic_robot_description/field_name", + this->nodeHandle->get_parameter("body_model/dynamic_robot_description/field_name", this->robotDescriptionUpdatesFieldName); - this->nodeHandle.get_parameter("sensor/point_by_point", this->pointByPointScan); + this->nodeHandle->get_parameter("sensor/point_by_point", this->pointByPointScan); // subscribe for robot_description param changes - this->robotDescriptionUpdatesListener = this->nodeHandle.template create_subscription( + this->robotDescriptionUpdatesListener = this->nodeHandle->template create_subscription( "dynamic_robot_model_server/parameter_updates", 10, std::bind(&RobotBodyFilter::robotDescriptionUpdated, this, std::placeholders::_1)); - // this->reloadRobotModelServiceServer = this->nodeHandle.template create_service( + // this->reloadRobotModelServiceServer = this->nodeHandle->template create_service( // "reload_model", std::bind(&RobotBodyFilter::triggerModelReload, this, std::placeholders::_1, std::placeholders::_2)); // if (this->computeBoundingSphere) { // this->boundingSpherePublisher = - // this->nodeHandle.template advertise( + // this->nodeHandle->template advertise( // "robot_bounding_sphere", 100); // } // if (this->computeBoundingBox) { - // this->boundingBoxPublisher = this->nodeHandle.template + // this->boundingBoxPublisher = this->nodeHandle->template // advertise("robot_bounding_box", // 100); // } // if (this->computeOrientedBoundingBox) { - // this->orientedBoundingBoxPublisher = this->nodeHandle.template + // this->orientedBoundingBoxPublisher = this->nodeHandle->template // advertise("robot_oriented_bounding_box", // 100); // } // if (this->computeLocalBoundingBox) { - // this->localBoundingBoxPublisher = this->nodeHandle.template + // this->localBoundingBoxPublisher = this->nodeHandle->template // advertise("robot_local_bounding_box", // 100); // } // if (this->publishBoundingSphereMarker && this->computeBoundingSphere) { - // this->boundingSphereMarkerPublisher = this->nodeHandle.template + // this->boundingSphereMarkerPublisher = this->nodeHandle->template // advertise("robot_bounding_sphere_marker", // 100); // } // if (this->publishBoundingBoxMarker && this->computeBoundingBox) { - // this->boundingBoxMarkerPublisher = this->nodeHandle.template + // this->boundingBoxMarkerPublisher = this->nodeHandle->template // advertise("robot_bounding_box_marker", // 100); // } // if (this->publishOrientedBoundingBoxMarker && // this->computeOrientedBoundingBox) { - // this->orientedBoundingBoxMarkerPublisher = this->nodeHandle.template + // this->orientedBoundingBoxMarkerPublisher = this->nodeHandle->template // advertise("robot_oriented_bounding_box_marker", // 100); // } // if (this->publishLocalBoundingBoxMarker && // this->computeLocalBoundingBox) { - // this->localBoundingBoxMarkerPublisher = this->nodeHandle.template + // this->localBoundingBoxMarkerPublisher = this->nodeHandle->template // advertise("robot_local_bounding_box_marker", // 100); // } @@ -447,7 +447,7 @@ bool RobotBodyFilter::configure() { // if (this->publishNoBoundingBoxPointcloud) // { // this->scanPointCloudNoBoundingBoxPublisher = - // this->nodeHandle.template + // this->nodeHandle->template // advertise("scan_point_cloud_no_bbox", // 100); // } @@ -455,7 +455,7 @@ bool RobotBodyFilter::configure() { // if (this->publishNoOrientedBoundingBoxPointcloud) // { // this->scanPointCloudNoOrientedBoundingBoxPublisher = - // this->nodeHandle.template + // this->nodeHandle->template // advertise("scan_point_cloud_no_oriented_bbox", // 100); // } @@ -463,7 +463,7 @@ bool RobotBodyFilter::configure() { // if (this->publishNoLocalBoundingBoxPointcloud) // { // this->scanPointCloudNoLocalBoundingBoxPublisher = - // this->nodeHandle.template + // this->nodeHandle->template // advertise("scan_point_cloud_no_local_bbox", // 100); // } @@ -471,82 +471,82 @@ bool RobotBodyFilter::configure() { // if (this->publishNoBoundingSpherePointcloud) // { // this->scanPointCloudNoBoundingSpherePublisher = - // this->nodeHandle.template + // this->nodeHandle->template // advertise("scan_point_cloud_no_bsphere", // 100); // } // if (this->publishDebugPclInside) // { - // this->debugPointCloudInsidePublisher = this->nodeHandle.template + // this->debugPointCloudInsidePublisher = this->nodeHandle->template // advertise("scan_point_cloud_inside", // 100); // } // if (this->publishDebugPclClip) // { - // this->debugPointCloudClipPublisher = this->nodeHandle.template + // this->debugPointCloudClipPublisher = this->nodeHandle->template // advertise("scan_point_cloud_clip", // 100); // } // if (this->publishDebugPclShadow) // { - // this->debugPointCloudShadowPublisher = this->nodeHandle.template + // this->debugPointCloudShadowPublisher = this->nodeHandle->template // advertise("scan_point_cloud_shadow", // 100); // } // if (this->publishDebugContainsMarker) // { - // this->debugContainsMarkerPublisher = this->nodeHandle.template + // this->debugContainsMarkerPublisher = this->nodeHandle->template // advertise("robot_model_for_contains_test", // 100); // } // if (this->publishDebugShadowMarker) // { - // this->debugShadowMarkerPublisher = this->nodeHandle.template + // this->debugShadowMarkerPublisher = this->nodeHandle->template // advertise("robot_model_for_shadow_test", // 100); // } // if (this->publishDebugBsphereMarker) // { - // this->debugBsphereMarkerPublisher = this->nodeHandle.template + // this->debugBsphereMarkerPublisher = this->nodeHandle->template // advertise("robot_model_for_bounding_sphere", // 100); // } // if (this->publishDebugBboxMarker) // { - // this->debugBboxMarkerPublisher = this->nodeHandle.template + // this->debugBboxMarkerPublisher = this->nodeHandle->template // advertise("robot_model_for_bounding_box", // 100); // } // if (this->computeDebugBoundingBox) { - // this->boundingBoxDebugMarkerPublisher = this->nodeHandle.template + // this->boundingBoxDebugMarkerPublisher = this->nodeHandle->template // advertise( // "robot_bounding_box_debug", 100); // } // if (this->computeDebugOrientedBoundingBox) { // this->orientedBoundingBoxDebugMarkerPublisher = - // this->nodeHandle.template + // this->nodeHandle->template // advertise( // "robot_oriented_bounding_box_debug", 100); // } // if (this->computeDebugLocalBoundingBox) { // this->localBoundingBoxDebugMarkerPublisher = - // this->nodeHandle.template + // this->nodeHandle->template // advertise( // "robot_local_bounding_box_debug", 100); // } // if (this->computeDebugBoundingSphere) { - // this->boundingSphereDebugMarkerPublisher = this->nodeHandle.template + // this->boundingSphereDebugMarkerPublisher = this->nodeHandle->template // advertise( // "robot_bounding_sphere_debug", 100); // } @@ -568,26 +568,23 @@ bool RobotBodyFilter::configure() { { initialMonitoredFrames.insert(this->sensorFrame); } - // auto rateVar = rclcpp::Duration::from_seconds(1.0).nanoseconds(); - // auto loop_rate = rclcpp::Rate(rateVar); - // this->tfFramesWatchdog = - // std::make_shared(this->filteringFrame, - // initialMonitoredFrames, this->tfBuffer, - // this->unreachableTransformTimeout, - // loop_rate); + auto loop_rate = std::make_shared(rclcpp::Duration::from_seconds(1.0).nanoseconds()); + this->tfFramesWatchdog = + std::make_shared(this->nodeHandle, this->filteringFrame, initialMonitoredFrames, + this->tfBuffer, this->unreachableTransformTimeout, loop_rate); this->tfFramesWatchdog->start(); } { // initialize the robot body to be masked out std::string robotUrdf; - while (!this->nodeHandle.get_parameter(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { + while (!this->nodeHandle->get_parameter(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { if (this->failWithoutRobotDescription) { throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); } if (!rclcpp::ok()) return false; - RCLCPP_ERROR(this->nodeHandle.get_logger(), + RCLCPP_ERROR(this->nodeHandle->get_logger(), "RobotBodyFilter: %s is empty or not set. Please, provide " "the robot model. Waiting 1s. ", robotDescriptionParam.c_str()); @@ -601,54 +598,54 @@ bool RobotBodyFilter::configure() { this->addRobotMaskFromUrdf(robotUrdf); } - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Successfullyconfigured."); - RCLCPP_INFO(nodeHandle.get_logger(), "Filtering data inframe %s", this->filteringFrame.c_str()); - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Filtering into the following categories:"); - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tOUTSIDE"); - if (doClipping) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tCLIP"); - if (doContainsTest) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tINSIDE"); - if (doShadowTest) RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: \tSHADOW"); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Successfullyconfigured."); + RCLCPP_INFO(nodeHandle->get_logger(), "Filtering data inframe %s", this->filteringFrame.c_str()); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Filtering into the following categories:"); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: \tOUTSIDE"); + if (doClipping) RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: \tCLIP"); + if (doContainsTest) RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: \tINSIDE"); + if (doShadowTest) RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: \tSHADOW"); if (this->onlyLinks.empty()) { if (this->linksIgnoredEverywhere.empty()) { - RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to all links."); + RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to all links."); } else { - RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to all links except %s.", + RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to all links except %s.", to_string(this->linksIgnoredEverywhere).c_str()); } } else { if (this->linksIgnoredEverywhere.empty()) { - RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); + RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); } else { - RCLCPP_INFO(nodeHandle.get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", + RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", to_string(this->onlyLinks).c_str(), to_string(this->linksIgnoredEverywhere).c_str()); } } - this->timeConfigured = this->nodeHandle.now(); + this->timeConfigured = this->nodeHandle->now(); return true; } bool RobotBodyFilterLaserScan::configure() { - this->nodeHandle.declare_parameter("sensor/point_by_point", true); + this->nodeHandle->declare_parameter("sensor/point_by_point", true); this->pointByPointScan; bool success = RobotBodyFilter::configure(); return false; } bool RobotBodyFilterPointCloud2::configure() { - this->nodeHandle.declare_parameter("sensor/point_by_point", false); + this->nodeHandle->declare_parameter("sensor/point_by_point", false); bool success = RobotBodyFilter::configure(); if (!success) return false; - this->nodeHandle.get_parameter("frames/output", this->outputFrame); + this->nodeHandle->get_parameter("frames/output", this->outputFrame); std::vector tempPointChannels; std::vector tempDirectionChannels; - this->nodeHandle.get_parameter("cloud/point_channels", tempPointChannels); - this->nodeHandle.get_parameter("cloud/direction_channels", tempDirectionChannels); + this->nodeHandle->get_parameter("cloud/point_channels", tempPointChannels); + this->nodeHandle->get_parameter("cloud/direction_channels", tempDirectionChannels); const auto pointChannels = tempPointChannels; const auto directionChannels = tempDirectionChannels; @@ -680,7 +677,7 @@ bool RobotBodyFilter::computeMask( remainingTime(scanTime, this->reachableTransformTimeout)); tf2::fromMsg(sensorTf.transform.translation, sensorPosition); } catch (tf2::TransformException& e) { - RCLCPP_ERROR(nodeHandle.get_logger(), + RCLCPP_ERROR(nodeHandle->get_logger(), "RobotBodyFilter: Could not compute filtering mask due to this " "TF exception: %s", e.what()); @@ -726,7 +723,7 @@ bool RobotBodyFilter::computeMask( // isn't really taken point by point with different timestamps if (scanDuration == 0.0) { updateBodyPosesEvery = num_points(projectedPointCloud) + 1; - RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + RCLCPP_WARN_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: sensor/point_by_point is set to true but " "all points in the cloud have the same timestamp. You should" " change the parameter to false to gain performance."); @@ -763,7 +760,7 @@ bool RobotBodyFilter::computeMask( } } - RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); this->publishDebugPointClouds(projectedPointCloud, pointMask); @@ -773,7 +770,7 @@ bool RobotBodyFilter::computeMask( this->computeAndPublishOrientedBoundingBox(projectedPointCloud); this->computeAndPublishLocalBoundingBox(projectedPointCloud); - RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); return true; } @@ -784,7 +781,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc const auto& scanTime = rclcpp::Time(inputScan.header.stamp); if (!this->configured_) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); @@ -792,7 +789,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc } if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) >= timeConfigured)) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); @@ -800,7 +797,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc } if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) < timeConfigured)) { - RCLCPP_WARN(nodeHandle.get_logger(), + RCLCPP_WARN(nodeHandle->get_logger(), "RobotBodyFilter: Old TF data received. Clearing TF buffer and " "reconfiguring laser" "filter. If you're replaying a bag file, make sure rosparam " @@ -816,7 +813,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc // Passing a sensorFrame does not make sense. Scan messages can't be // transformed to other frames. if (!this->sensorFrame.empty() && this->sensorFrame != scanFrame) { - RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + RCLCPP_WARN_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: frames/sensor is set to frame_id '%s' different than " "the frame_id of the incoming message '%s'. This is an invalid " "configuration: " @@ -825,7 +822,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc } if (!this->tfFramesWatchdog->isReachable(scanFrame)) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Throwing away scan since sensor frame is " "unreachable."); // if this->sensorFrame is empty, it can happen that we're not actually @@ -835,7 +832,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc } if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable()) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Throwing away scan since not all frames are " "reachable."); return false; @@ -864,15 +861,15 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc !this->tfBuffer->canTransform(this->fixedFrame, scanFrame, afterScanTime, remainingTime(afterScanTime, this->reachableTransformTimeout), &err)) { if (err.find("future") != std::string::npos) { - const auto delay = nodeHandle.now() - scanTime; - auto& clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + const auto delay = nodeHandle->now() - scanTime; + auto& clk = *nodeHandle->get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform laser scan to " "fixed frame. The scan is too much delayed (%s s). TF error: %s", to_string(delay).c_str(), err.c_str()); } else { - auto& clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + auto& clk = *nodeHandle->get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform laser scan to " "fixed frame. Something's wrong with TFs: %s", err.c_str()); @@ -894,14 +891,14 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc auto channelOptions = laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Index; if (this->pointByPointScan) { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Applying complex laser scan projection."); + RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Applying complex laser scan projection."); // perform the complex laser scan projection channelOptions |= laser_geometry::channel_option::Timestamp | laser_geometry::channel_option::Viewpoint; laserProjector.transformLaserScanToPointCloud(this->fixedFrame, inputScan, tmpPointCloud, *this->tfBuffer, -1, channelOptions); } else { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Applying simple laser scan projection."); + RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Applying simple laser scan projection."); // perform simple laser scan projection laserProjector.projectLaser(inputScan, tmpPointCloud, -1.0, channelOptions); } @@ -910,13 +907,13 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc if (tmpPointCloud.header.frame_id == this->filteringFrame) { projectedPointCloud = std::move(tmpPointCloud); } else { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Transforming scan from frame %s to %s", + RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Transforming scan from frame %s to %s", tmpPointCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); std::string err; if (!this->tfBuffer->canTransform(this->filteringFrame, tmpPointCloud.header.frame_id, scanTime, remainingTime(scanTime, this->reachableTransformTimeout), &err)) { // RCLCPP_ERROR_DELAYED_THROTTLE( - // nodeHandle.get_logger(), 3, + // nodeHandle->get_logger(), 3, // "RobotBodyFilter: Cannot transform " // "laser scan to filtering frame. Something's wrong with TFs: // %s", err.c_str()); @@ -928,7 +925,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc } } - RCLCPP_DEBUG(nodeHandle.get_logger(), "RobotBodyFilter: Scan transformation run time is %.5f secs.", + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Scan transformation run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); std::vector pointMask; @@ -956,7 +953,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc ++indexIt; } } catch (std::runtime_error& ) { - RCLCPP_ERROR(this->nodeHandle.get_logger(), + RCLCPP_ERROR(this->nodeHandle->get_logger(), "RobotBodyFilter: projectedPointCloud doesn't have field " "called 'index'," " but the algorithm relies on that."); @@ -974,7 +971,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp const auto& scanTime = rclcpp::Time(headerScanTime); if (!this->configured_) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); @@ -982,7 +979,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) >= this->timeConfigured)) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); @@ -990,7 +987,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) < this->timeConfigured)) { - RCLCPP_WARN(nodeHandle.get_logger(), + RCLCPP_WARN(nodeHandle->get_logger(), "RobotBodyFilter: Old TF data received. Clearing TF buffer and " "reconfiguring laser filter. If you're replaying a bag file, make " "sure rosparam /use_sim_time is set to true"); @@ -1002,7 +999,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp this->sensorFrame.empty() ? stripLeadingSlash(inputCloud.header.frame_id, true) : this->sensorFrame; if (!this->tfFramesWatchdog->isReachable(inputCloudFrame)) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Throwing away scan since sensor frame is " "unreachable."); // if this->sensorFrame is empty, it can happen that we're not actually @@ -1013,7 +1010,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } if (this->requireAllFramesReachable && !this->tfFramesWatchdog->areAllFramesReachable()) { - RCLCPP_DEBUG(nodeHandle.get_logger(), + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Throwing away scan since not all frames are " "reachable."); return false; @@ -1036,7 +1033,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp if (this->pointByPointScan) { if (inputCloud.height != 1 && inputCloud.is_dense == 0) { - RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + RCLCPP_WARN_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: The pointcloud seems to be an organized " "pointcloud, which usually means it was captured all at once." " Consider setting 'point_by_point_scan' to false to get a " @@ -1048,13 +1045,13 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp "fields 'stamps', 'vp_x', 'vp_y' and 'vp_z'."); } } else if (hasStampsField) { - RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + RCLCPP_WARN_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: The pointcloud has a 'stamps' field, " "which indicates each point was probably captured at a " "different time instant. Consider setting parameter " "'point_by_point_scan' to true to get correct results."); } else if (inputCloud.height == 1 && inputCloud.is_dense == 1) { - RCLCPP_WARN_ONCE(nodeHandle.get_logger(), + RCLCPP_WARN_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: The pointcloud is dense, which usually means" " it was captured each point at a different time instant. " "Consider setting 'point_by_point_scan' to true to get a more" @@ -1067,14 +1064,14 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp if (inputCloud.header.frame_id == this->filteringFrame) { transformedCloud = inputCloud; } else { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", + RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; if (!this->tfBuffer->canTransform(this->filteringFrame, inputCloud.header.frame_id, scanTime, remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - auto& clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + auto& clk = *nodeHandle->get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform " "point cloud to filtering frame. Something's wrong with TFs: %s", err.c_str()); @@ -1106,14 +1103,14 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp if (tmpCloud.header.frame_id == this->outputFrame) { filteredCloud = std::move(tmpCloud); } else { - RCLCPP_INFO_ONCE(nodeHandle.get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", + RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", tmpCloud.header.frame_id.c_str(), this->outputFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; if (!this->tfBuffer->canTransform(this->outputFrame, tmpCloud.header.frame_id, scanTime, remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - auto& clk = *nodeHandle.get_clock(); - RCLCPP_ERROR_THROTTLE(nodeHandle.get_logger(), clk, 3, + auto& clk = *nodeHandle->get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform " "point cloud to output frame. Something's wrong with TFs: %s", err.c_str()); @@ -1229,7 +1226,7 @@ void RobotBodyFilter::updateTransformCache(const rclcpp::Time& time, const rc template void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { if (urdfModel.empty()) { - RCLCPP_ERROR(nodeHandle.get_logger(), + RCLCPP_ERROR(nodeHandle->get_logger(), "RobotBodyFilter: Empty string passed as robot model to " "addRobotMaskFromUrdf. " "Robot body filtering is not going to work."); @@ -1240,7 +1237,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { urdf::Model parsedUrdfModel; bool urdfParseSucceeded = parsedUrdfModel.initString(urdfModel); // if (!urdfParseSucceeded) { - // RCLCPP_ERROR(nodeHandle.get_logger(),"RobotBodyFilter: The URDF model + // RCLCPP_ERROR(nodeHandle->get_logger(),"RobotBodyFilter: The URDF model // given in parameter '" // << this->robotDescriptionParam // << "' cannot be parsed. See " @@ -1266,7 +1263,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { size_t collisionIndex = 0; for (const auto& collision : link->collision_array) { if (collision->geometry == nullptr) { - RCLCPP_WARN(nodeHandle.get_logger(), + RCLCPP_WARN(nodeHandle->get_logger(), "RobotBodyFilter: Collision element without geometry found " "in link %s of robot %s. " "This collision element will not be filtered out.", @@ -1316,7 +1313,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { // if the shape could not be constructed, ignore it (e.g. mesh was not // found) if (collisionShape == nullptr) { - RCLCPP_WARN(nodeHandle.get_logger(), "Could not construct shape for collision %s, ignoring it.", + RCLCPP_WARN(nodeHandle->get_logger(), "Could not construct shape for collision %s, ignoring it.", shapeName.c_str()); ++collisionIndex; continue; @@ -1360,7 +1357,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { if (collisionIndex == 0 && !link->visual_array.empty()) { if ((this->onlyLinks.empty() || (this->onlyLinks.find(link->name) != this->onlyLinks.end())) && this->linksIgnoredEverywhere.find(link->name) == this->linksIgnoredEverywhere.end()) { - RCLCPP_WARN(nodeHandle.get_logger(), + RCLCPP_WARN(nodeHandle->get_logger(), "RobotBodyFilter: No collision element found for link %s of " "robot %s. This link will not be filtered out " "from laser scans.", @@ -1832,12 +1829,12 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( try { if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - RCLCPP_ERROR(nodeHandle.get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), + RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), this->localBoundingBoxFrame.c_str(), err.c_str()); return; } } catch (tf2::TransformException& e) { - RCLCPP_ERROR(nodeHandle.get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), + RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), this->localBoundingBoxFrame.c_str(), e.what()); return; } @@ -1988,7 +1985,7 @@ void RobotBodyFilter::robotDescriptionUpdated(const std_msgs::msg::String::Sh auto urdf = newConfig; - RCLCPP_INFO(nodeHandle.get_logger(), + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Reloading robot model because of " "dynamic_reconfigure update. Filter operation stopped."); @@ -1999,23 +1996,23 @@ void RobotBodyFilter::robotDescriptionUpdated(const std_msgs::msg::String::Sh this->addRobotMaskFromUrdf(urdf); this->tfFramesWatchdog->unpause(); - this->timeConfigured = nodeHandle.now(); + this->timeConfigured = nodeHandle->now(); this->configured_ = true; - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); } template bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request&, std_srvs::srv::Trigger_Response&) { std::string urdf; - auto success = this->nodeHandle.getParam(this->robotDescriptionParam, urdf); + auto success = this->nodeHandle->getParam(this->robotDescriptionParam, urdf); if (!success) { ROS_ERROR_STREAM("RobotBodyFilter: Parameter " << this->robotDescriptionParam << " doesn't exist."); return false; } - RCLCPP_INFO(nodeHandle.get_logger(), + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Reloading robot model because of trigger. Filter " "operation stopped."); @@ -2026,10 +2023,10 @@ bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request&, std this->addRobotMaskFromUrdf(urdf); this->tfFramesWatchdog->unpause(); - this->timeConfigured = nodeHandle.now(); + this->timeConfigured = nodeHandle->now(); this->configured_ = true; - RCLCPP_INFO(nodeHandle.get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); return true; } diff --git a/src/TFFramesWatchdog.cpp b/src/TFFramesWatchdog.cpp index e4598fc..17a8cb8 100644 --- a/src/TFFramesWatchdog.cpp +++ b/src/TFFramesWatchdog.cpp @@ -13,7 +13,7 @@ TFFramesWatchdog::TFFramesWatchdog( std::shared_ptr tfBuffer, rclcpp::Duration unreachableTfLookupTimeout, rclcpp::Rate::SharedPtr unreachableFramesCheckRate): - nodeHandle(std::move(nodeHandle)), + nodeHandle(nodeHandle), robotFrame(std::move(robotFrame)), monitoredFrames(std::move(monitoredFrames)), tfBuffer(tfBuffer), From 46d7aac9f3ee6092e9244c6ce8ca14840d97454b Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 18 Apr 2024 18:35:44 -0400 Subject: [PATCH 27/54] error msgs and computeBoundingBox --- src/RobotBodyFilter.cpp | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index e485ed2..227fdbb 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -912,11 +912,11 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc std::string err; if (!this->tfBuffer->canTransform(this->filteringFrame, tmpPointCloud.header.frame_id, scanTime, remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - // RCLCPP_ERROR_DELAYED_THROTTLE( - // nodeHandle->get_logger(), 3, - // "RobotBodyFilter: Cannot transform " - // "laser scan to filtering frame. Something's wrong with TFs: - // %s", err.c_str()); + auto& clk = *nodeHandle->get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, + "RobotBodyFilter: Cannot transform " + "laser scan to filtering frame. Something's wrong with TFs: %s", + err.c_str()); return false; } @@ -1131,9 +1131,9 @@ bool RobotBodyFilter::getShapeTransform(point_containment_filter::ShapeHandle // check if the given shapeHandle has been registered to a link during // addRobotMaskFromUrdf call. if (this->shapesToLinks.find(shapeHandle) == this->shapesToLinks.end()) { - // RCLCPP_ERROR_THROTTLE( - // 3, "RobotBodyFilter: Invalid shape handle: " << - // to_string(shapeHandle)); + auto& clk = *nodeHandle->get_clock(); + RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, + 3, "RobotBodyFilter: Invalid shape handle: %s", to_string(shapeHandle).c_str()); return false; } @@ -1717,10 +1717,13 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( const auto& shapeHandle = shapeHandleAndBody.first; const auto& body = shapeHandleAndBody.second; - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; + if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) + continue; + //TODO: SOLVE THIS bodies::OrientedBoundingBox box; - // TODO: This is probabbly wrong, originally computeBoundingBox took in + // body->computeBoundingBox(box); + // This is probabbly wrong, originally computeBoundingBox took in // the OrientedBoundingBox directly but I can't get that to work auto aaba = box.toAABB(); body->computeBoundingBox(aaba); From d316ea260c96a26de5094a8b3b4b17137ba4f9e0 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 10:19:48 -0400 Subject: [PATCH 28/54] Added more tests --- CMakeLists.txt | 60 +++++++++++++++++++++----------------------- src/utils/shapes.cpp | 6 ++--- 2 files changed, 31 insertions(+), 35 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 67ed6f7..34f11c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,18 +26,14 @@ find_package(Threads REQUIRED) find_package(PkgConfig REQUIRED) find_package(rosidl_default_generators REQUIRED) -rosidl_generate_interfaces(${PROJECT_NAME} "msg/OrientedBoundingBox.msg" "msg/OrientedBoundingBoxStamped.msg" DEPENDENCIES ${MESSAGE_DEPS}) +rosidl_generate_interfaces(${PROJECT_NAME}_msgs "msg/OrientedBoundingBox.msg" "msg/OrientedBoundingBoxStamped.msg" DEPENDENCIES ${MESSAGE_DEPS} LIBRARY_NAME ${PROJECT_NAME}) -include_directories( - include - include/utils - ${Dependency}_INCLUDE_DIRS -) +include_directories(include) set(UTILS_SRCS src/utils/bodies.cpp # src/utils/cloud.cpp -# src/utils/shapes.cpp + src/utils/shapes.cpp # src/utils/string_utils.cpp # src/utils/tf2_eigen.cpp # src/utils/tf2_sensor_msgs.cpp @@ -45,37 +41,34 @@ set(UTILS_SRCS ) add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) -ament_target_dependencies(${PROJECT_NAME}_utils -${THIS_PACKAGE_DEPS} -) +ament_target_dependencies(${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS}) -add_library(lib${PROJECT_NAME} src/RobotBodyFilter.cpp) -target_link_libraries(lib${PROJECT_NAME} - ${PROJECT_NAME}_utils -) -ament_target_dependencies(lib${PROJECT_NAME} - ${THIS_PACKAGE_DEPS} -) +add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) +target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils) +# ament_target_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS}) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include) -target_include_directories(lib${PROJECT_NAME} - PUBLIC - $ - $ - ${THIS_PACKAGE_INCLUDE_DEPENDS} +install(TARGETS ${PROJECT_NAME}_utils + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib ) if (BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) - # ament_add_gtest(test_set_utils test/test_set_utils.cpp) - # ament_target_dependencies(test_set_utils ${THIS_PACKAGE_DEPS}) - # target_link_libraries(test_set_utils lib${PROJECT_NAME}) + ament_add_gtest(test_set_utils test/test_set_utils.cpp) + target_link_libraries(test_set_utils ${PROJECT_NAME}_utils) + ament_target_dependencies(test_set_utils ${THIS_PACKAGE_DEPS}) -# ament_add_gtest(test_shapes_rbf test/test_shapes.cpp) -# target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + ament_add_gtest(test_shapes_rbf test/test_shapes.cpp) + target_link_libraries(test_shapes_rbf ${PROJECT_NAME}_utils) + ament_target_dependencies(test_shapes_rbf ${THIS_PACKAGE_DEPS}) -# ament_add_gtest(test_urdf_eigen test/test_urdf_eigen.cpp) -# target_link_libraries(test_urdf_eigen ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) + # ament_add_gtest(test_urdf_eigen test/test_urdf_eigen.cpp) + # target_link_libraries(test_urdf_eigen ${PROJECT_NAME}_utils) # ament_add_gtest(test_time_utils test/test_time_utils.cpp) # target_link_libraries(test_time_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) @@ -86,9 +79,9 @@ if (BUILD_TESTING) # ament_add_gtest(test_string_utils test/test_string_utils.cpp) # target_link_libraries(test_string_utils ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) - ament_add_gtest(test_bodies test/test_bodies.cpp) - ament_target_dependencies(test_bodies ${THIS_PACKAGE_DEPS}) - target_link_libraries(test_bodies lib${PROJECT_NAME} ${PROJECT_NAME}_utils) + # ament_add_gtest(test_bodies test/test_bodies.cpp) + # ament_target_dependencies(test_bodies ${THIS_PACKAGE_DEPS}) + # target_link_libraries(test_bodies ${PROJECT_NAME} ${PROJECT_NAME}_utils) # ament_add_gtest(test_cloud test/test_cloud.cpp) # target_link_libraries(test_cloud ${PROJECT_NAME}_utils ${catkin_LIBRARIES}) @@ -117,6 +110,9 @@ if (BUILD_TESTING) # ament_add_gtest(test_robot_body_filter test/test_robot_body_filter.test test/test_robot_body_filter.cpp) # target_link_libraries(test_robot_body_filter ${PROJECT_NAME} ${catkin_LIBRARIES}) # target_compile_options(test_robot_body_filter PRIVATE -fno-var-tracking-assignments) # speed up build + + install(FILES test/box.dae test/triangle.dae + DESTINATION share/robot_body_filter/test) endif() ament_export_dependencies(${THIS_PACKAGE_DEPS}) diff --git a/src/utils/shapes.cpp b/src/utils/shapes.cpp index 2480e48..520c657 100644 --- a/src/utils/shapes.cpp +++ b/src/utils/shapes.cpp @@ -1,6 +1,6 @@ #include -#include +#include #include #include #include @@ -30,11 +30,11 @@ shapes::ShapeConstPtr robot_body_filter::constructShape(const urdf::Geometry& ge Eigen::Vector3d scale(mesh->scale.x, mesh->scale.y, mesh->scale.z); result.reset(shapes::createMeshFromResource(mesh->filename, scale)); } else - ROS_WARN("Empty mesh filename"); + RCLCPP_WARN(rclcpp::get_logger("shapes"), "Empty mesh filename"); break; } default: { - ROS_ERROR("Unknown geometry type: %d", (int) geometry.type); + RCLCPP_ERROR(rclcpp::get_logger("shapes"), "Unknown geometry type: %d", (int) geometry.type); break; } } From 95bc2c7efde10822f1dcf9a267698cf91b6891ee Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 10:28:49 -0400 Subject: [PATCH 29/54] Another test --- CMakeLists.txt | 13 ++++++++----- src/utils/cloud.cpp | 22 +++++++++++----------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 34f11c2..667cb43 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,12 +32,12 @@ include_directories(include) set(UTILS_SRCS src/utils/bodies.cpp -# src/utils/cloud.cpp + # src/utils/cloud.cpp src/utils/shapes.cpp # src/utils/string_utils.cpp -# src/utils/tf2_eigen.cpp -# src/utils/tf2_sensor_msgs.cpp -# src/utils/time_utils.cpp + src/utils/tf2_eigen.cpp + # src/utils/tf2_sensor_msgs.cpp + # src/utils/time_utils.cpp ) add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) @@ -50,12 +50,15 @@ target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include) -install(TARGETS ${PROJECT_NAME}_utils +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib ) +install(FILES rviz/debug.rviz + DESTINATION share/rviz) + if (BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) diff --git a/src/utils/cloud.cpp b/src/utils/cloud.cpp index 2490384..39e9dbb 100644 --- a/src/utils/cloud.cpp +++ b/src/utils/cloud.cpp @@ -1,6 +1,6 @@ #include #define private protected -#include +#include #undef private #include @@ -21,7 +21,7 @@ bool hasField(const Cloud& cloud, const std::string& fieldName) { return false; } -sensor_msgs::PointField& getField(Cloud& cloud, const std::string& fieldName) { +sensor_msgs::msg::PointField& getField(Cloud& cloud, const std::string& fieldName) { for (auto& field : cloud.fields) { if (field.name == fieldName) return field; @@ -29,7 +29,7 @@ sensor_msgs::PointField& getField(Cloud& cloud, const std::string& fieldName) { throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); } -const sensor_msgs::PointField& getField(const Cloud& cloud, +const sensor_msgs::msg::PointField& getField(const Cloud& cloud, const std::string& fieldName) { for (const auto& field : cloud.fields) { if (field.name == fieldName) @@ -38,21 +38,21 @@ const sensor_msgs::PointField& getField(const Cloud& cloud, throw std::runtime_error(std::string("Field ") + fieldName + " does not exist."); } -size_t sizeOfPointField(const sensor_msgs::PointField& field) +size_t sizeOfPointField(const sensor_msgs::msg::PointField& field) { return sizeOfPointField(field.datatype); } size_t sizeOfPointField(int datatype) { - if ((datatype == sensor_msgs::PointField::INT8) || (datatype == sensor_msgs::PointField::UINT8)) + if ((datatype == sensor_msgs::msg::PointField::INT8) || (datatype == sensor_msgs::msg::PointField::UINT8)) return 1u; - else if ((datatype == sensor_msgs::PointField::INT16) || (datatype == sensor_msgs::PointField::UINT16)) + else if ((datatype == sensor_msgs::msg::PointField::INT16) || (datatype == sensor_msgs::msg::PointField::UINT16)) return 2u; - else if ((datatype == sensor_msgs::PointField::INT32) || (datatype == sensor_msgs::PointField::UINT32) || - (datatype == sensor_msgs::PointField::FLOAT32)) + else if ((datatype == sensor_msgs::msg::PointField::INT32) || (datatype == sensor_msgs::msg::PointField::UINT32) || + (datatype == sensor_msgs::msg::PointField::FLOAT32)) return 4u; - else if (datatype == sensor_msgs::PointField::FLOAT64) + else if (datatype == sensor_msgs::msg::PointField::FLOAT64) return 8u; else throw std::runtime_error(std::string("PointField of type ") + std::to_string(datatype) + " does not exist"); @@ -93,8 +93,8 @@ void GenericCloudIterator::copyData(const GenericCloudIterator& otherIter) } // explicitly instantiate -template class GenericCloudIteratorBase; -template class GenericCloudIteratorBase; +template class GenericCloudIteratorBase; +template class GenericCloudIteratorBase; template class GenericCloudIterator; template class GenericCloudConstIterator; } From 111757692844a3cc8225f975d81f24015316c8f3 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 10:32:30 -0400 Subject: [PATCH 30/54] Fixed added pcl to package.xml, and allow CI to jobs to continue if others fail --- .github/workflows/main.yml | 2 ++ package.xml | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5482982..f97447e 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -8,6 +8,8 @@ on: [push, pull_request] # on all pushes and PRs jobs: industrial_ci: + continue-on-error: true + strategy: matrix: env: diff --git a/package.xml b/package.xml index 65d677b..5373cda 100644 --- a/package.xml +++ b/package.xml @@ -21,7 +21,7 @@ geometric_shapes geometry_msgs laser_geometry - + libpcl-all-dev moveit_core moveit_ros_perception rclcpp From eb4e75d9f775858a79aed37d38db945c7ed29e36 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 10:56:45 -0400 Subject: [PATCH 31/54] Add rosidl_default_generators to package.xml --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 5373cda..2212571 100644 --- a/package.xml +++ b/package.xml @@ -37,6 +37,7 @@ tf2_eigen tf2_sensor_msgs + rosidl_default_generators rosidl_default_runtime rosidl_interface_packages From f6f479eadf920c666cd79fcc92226c8b26ffe0c4 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 16:31:17 -0400 Subject: [PATCH 32/54] Fixed obb --- CMakeLists.txt | 10 ++++++-- src/RayCastingShapeMask.cpp | 49 +++++++++++++++++++------------------ src/RobotBodyFilter.cpp | 7 +----- 3 files changed, 34 insertions(+), 32 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 667cb43..346af87 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,9 +43,15 @@ add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS}) +add_library(RayCastingShapeMask src/RayCastingShapeMask.cpp) +target_link_libraries(RayCastingShapeMask ${PROJECT_NAME}_utils) + add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) -target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils) -# ament_target_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS}) +target_link_libraries(${PROJECT_NAME} + ${PROJECT_NAME}_utils + RayCastingShapeMask +) +ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include) diff --git a/src/RayCastingShapeMask.cpp b/src/RayCastingShapeMask.cpp index d513492..88ae19a 100644 --- a/src/RayCastingShapeMask.cpp +++ b/src/RayCastingShapeMask.cpp @@ -12,7 +12,7 @@ #include -#include +// #include namespace robot_body_filter { @@ -56,7 +56,7 @@ RayCastingShapeMask::~RayCastingShapeMask() = default; std::map RayCastingShapeMask::getBoundingSpheres() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map map; size_t bodyIndex = 0, sphereIndex = 0; @@ -77,7 +77,7 @@ RayCastingShapeMask::getBoundingSpheres() const std::map RayCastingShapeMask::getBoundingSpheresForContainsTest() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map map; size_t bodyIndex = 0, sphereIndex = 0; @@ -97,7 +97,7 @@ RayCastingShapeMask::getBoundingSpheresForContainsTest() const bodies::BoundingSphere RayCastingShapeMask::getBoundingSphere() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); return this->getBoundingSphereNoLock(); } @@ -113,7 +113,7 @@ bodies::BoundingSphere RayCastingShapeMask::getBoundingSphereForContainsTestNoLo void RayCastingShapeMask::updateBodyPoses() { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); this->updateBodyPosesNoLock(); } @@ -160,22 +160,23 @@ void RayCastingShapeMask::updateBodyPosesNoLock() } else { - if (containsBody == nullptr) - ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", - "Missing transform for shape with handle " << containsHandle - << " without a body"); + if (containsBody == nullptr){ + // ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", + // "Missing transform for shape with handle " << containsHandle + // << " without a body"); + } else { std::string name; if (this->data->shapeNames.find(containsHandle) != this->data->shapeNames.end()) name = this->data->shapeNames.at(containsHandle); - if (name.empty()) - ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", - "Missing transform for shape " << containsBody->getType() - << " with handle " << containsHandle); - else - ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", - "Missing transform for shape " << name << " (" << containsBody->getType() << ")"); + // if (name.empty()) + // ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", + // "Missing transform for shape " << containsBody->getType() + // << " with handle " << containsHandle); + // else + // ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", + // "Missing transform for shape " << name << " (" << containsBody->getType() << ")"); } } } @@ -224,7 +225,7 @@ void RayCastingShapeMask::maskContainmentAndShadows( const Cloud& data, std::vector& mask, const Eigen::Vector3d& sensorPos) { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); const auto np = num_points(data); mask.resize(np); @@ -258,7 +259,7 @@ void RayCastingShapeMask::maskContainmentAndShadows(const Eigen::Vector3f& data, return; } - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); if (updateBodyPoses) this->updateBodyPosesNoLock(); @@ -440,7 +441,7 @@ void RayCastingShapeMask::setTransformCallback( void RayCastingShapeMask::updateInternalShapeLists() { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); this->data->bodiesForContainsTest.clear(); this->data->bodiesForShadowTest.clear(); @@ -468,7 +469,7 @@ void RayCastingShapeMask::updateInternalShapeLists() std::map RayCastingShapeMask::getBodies() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map result; for (const auto& seeShape: this->bodies_) @@ -480,7 +481,7 @@ RayCastingShapeMask::getBodies() const std::map RayCastingShapeMask::getBodiesForContainsTest() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map result; for (const auto& seeShape: this->data->bodiesForContainsTest) @@ -492,7 +493,7 @@ RayCastingShapeMask::getBodiesForContainsTest() const std::map RayCastingShapeMask::getBodiesForShadowTest() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map result; for (const auto& seeShape: this->data->bodiesForShadowTest) @@ -504,7 +505,7 @@ RayCastingShapeMask::getBodiesForShadowTest() const std::map RayCastingShapeMask::getBodiesForBoundingSphere() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map result; for (const auto& seeShape: this->data->bodiesForBsphere) @@ -516,7 +517,7 @@ RayCastingShapeMask::getBodiesForBoundingSphere() const std::map RayCastingShapeMask::getBodiesForBoundingBox() const { - boost::mutex::scoped_lock _(this->shapes_lock_); + std::lock_guard _(this->shapes_lock_); std::map result; for (const auto& seeShape: this->data->bodiesForBbox) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 227fdbb..05d8b69 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1720,13 +1720,8 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; - //TODO: SOLVE THIS bodies::OrientedBoundingBox box; - // body->computeBoundingBox(box); - // This is probabbly wrong, originally computeBoundingBox took in - // the OrientedBoundingBox directly but I can't get that to work - auto aaba = box.toAABB(); - body->computeBoundingBox(aaba); + body->computeBoundingBox(box); boxes.push_back(box); From 196cc83094dae67ad8058f3977d43fe7218ae8ba Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Fri, 19 Apr 2024 16:40:37 -0400 Subject: [PATCH 33/54] Fixed more cmake stuff --- CMakeLists.txt | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 346af87..bcdaa15 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,12 +43,20 @@ add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS}) +add_library(tf2_sensor_msgs_rbf src/utils/tf2_sensor_msgs.cpp) +target_link_libraries(tf2_sensor_msgs_rbf PRIVATE ${PROJECT_NAME}_utils) + add_library(RayCastingShapeMask src/RayCastingShapeMask.cpp) target_link_libraries(RayCastingShapeMask ${PROJECT_NAME}_utils) +add_library(TFFramesWatchdog src/TFFramesWatchdog.cpp) +target_link_libraries(TFFramesWatchdog ${PROJECT_NAME}_utils) + add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils + tf2_sensor_msgs_rbf + TFFramesWatchdog RayCastingShapeMask ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) @@ -56,7 +64,7 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include) -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils +install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask ${PROJECT_NAME}_utils RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib From ea4d12dd106d77aa7b9a407891569728285e9523 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 19 Apr 2024 17:08:59 -0400 Subject: [PATCH 34/54] fixed warnings --- src/RobotBodyFilter.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 05d8b69..2b60297 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -12,13 +12,12 @@ #include #include -#include #include #include #include #include -#include +#include #include #include @@ -630,7 +629,6 @@ bool RobotBodyFilter::configure() { bool RobotBodyFilterLaserScan::configure() { this->nodeHandle->declare_parameter("sensor/point_by_point", true); - this->pointByPointScan; bool success = RobotBodyFilter::configure(); return false; } @@ -782,7 +780,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc if (!this->configured_) { RCLCPP_DEBUG(nodeHandle->get_logger(), - "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " + "RobotBodyFilter: Ignore scan from time %f.%ld - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); return false; @@ -790,7 +788,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc if ((scanTime < timeConfigured) && ((scanTime + tfBufferLength) >= timeConfigured)) { RCLCPP_DEBUG(nodeHandle->get_logger(), - "RobotBodyFilter: Ignore scan from time %u.%u - filter not yet " + "RobotBodyFilter: Ignore scan from time %f.%ld - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); return false; @@ -972,7 +970,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp if (!this->configured_) { RCLCPP_DEBUG(nodeHandle->get_logger(), - "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " + "RobotBodyFilter: Ignore cloud from time %f.%ld - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); return false; @@ -980,7 +978,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp if ((scanTime < this->timeConfigured) && ((scanTime + this->tfBufferLength) >= this->timeConfigured)) { RCLCPP_DEBUG(nodeHandle->get_logger(), - "RobotBodyFilter: Ignore cloud from time %u.%u - filter not yet " + "RobotBodyFilter: Ignore cloud from time %f.%ld - filter not yet " "initialized.", scanTime.seconds(), scanTime.nanoseconds()); return false; @@ -1684,7 +1682,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp @@ -1806,7 +1804,7 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp @@ -1933,7 +1931,7 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( pcl::PCLPointCloud2 pclOutput; cropBox.filter(pclOutput); - sensor_msgs::msg::PointCloud2::Ptr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); + sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp From c1051b91dcbcab89ab3d1514a0da55e746a2a69e Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 22 Apr 2024 13:18:35 -0400 Subject: [PATCH 35/54] removed local obb files --- include/robot_body_filter/utils/bodies.h | 1 - include/robot_body_filter/utils/obb.hpp | 148 ------------------ src/utils/obb.cpp | 183 ----------------------- 3 files changed, 332 deletions(-) delete mode 100644 include/robot_body_filter/utils/obb.hpp delete mode 100644 src/utils/obb.cpp diff --git a/include/robot_body_filter/utils/bodies.h b/include/robot_body_filter/utils/bodies.h index 9085e1f..5d90ac9 100644 --- a/include/robot_body_filter/utils/bodies.h +++ b/include/robot_body_filter/utils/bodies.h @@ -7,7 +7,6 @@ #if __has_include() #include #else -#include "obb.hpp" // #error not found. Please, update geometric_shapes // library to version 0.6.5+ or 0.7.4+ #endif diff --git a/include/robot_body_filter/utils/obb.hpp b/include/robot_body_filter/utils/obb.hpp deleted file mode 100644 index 30748ef..0000000 --- a/include/robot_body_filter/utils/obb.hpp +++ /dev/null @@ -1,148 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Open Robotics - * 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 Open Robotics 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: Martin Pecka */ - -#ifndef GEOMETRIC_SHAPES_OBB_H -#define GEOMETRIC_SHAPES_OBB_H - -#include -#include - -#include -#include - -#include -#include - -namespace bodies { -class OBBPrivate; - -/** \brief Represents an oriented bounding box. */ -class OBB { -public: - /** \brief Initialize an oriented bounding box at position 0, with 0 extents - * and identity orientation. */ - OBB(); - OBB(const OBB &other); - OBB(const Eigen::Isometry3d &pose, const Eigen::Vector3d &extents); - virtual ~OBB(); - - OBB &operator=(const OBB &other); - - /** - * \brief Set both the pose and extents of the OBB. - * \param [in] pose New pose of the OBB. - * \param [in] extents New extents of the OBB. - */ - void setPoseAndExtents(const Eigen::Isometry3d &pose, - const Eigen::Vector3d &extents); - - /** - * \brief Get the extents of the OBB. - * \return The extents. - */ - Eigen::Vector3d getExtents() const; - - /** - * \brief Get the extents of the OBB. - * \param extents [out] The extents. - */ - void getExtents(Eigen::Vector3d &extents) const; - - /** - * \brief Get the pose of the OBB. - * \return The pose. - */ - Eigen::Isometry3d getPose() const; - - /** - * \brief Get The pose of the OBB. - * \param pose The pose. - */ - void getPose(Eigen::Isometry3d &pose) const; - - /** - * \brief Convert this OBB to an axis-aligned BB. - * \return The AABB. - */ - AABB toAABB() const; - - /** - * \brief Convert this OBB to an axis-aligned BB. - * \param aabb The AABB. - */ - void toAABB(AABB &aabb) const; - - /** - * \brief Add the other OBB to this one and compute an approximate enclosing - * OBB. \param box The other box to add. \return Pointer to this OBB after the - * update. - */ - OBB *extendApprox(const OBB &box); - - /** - * \brief Check if this OBB contains the given point. - * \param point The point to check. - * \return Whether the point is inside or not. - */ - bool contains(const Eigen::Vector3d &point) const; - - /** - * \brief Check whether this and the given OBBs have nonempty intersection. - * \param other The other OBB to check. - * \return Whether the OBBs overlap. - */ - bool overlaps(const OBB &other) const; - - /** - * \brief Check if this OBB contains whole other OBB. - * \param point The point to check. - * \return Whether the point is inside or not. - */ - bool contains(const OBB &obb) const; - - /** - * \brief Compute coordinates of the 8 vertices of this OBB. - * \return The vertices. - */ - EigenSTL::vector_Vector3d computeVertices() const; - -protected: - /** \brief PIMPL pointer */ - std::unique_ptr obb_; -}; -} // namespace bodies - -#endif // GEOMETRIC_SHAPES_OBB_H \ No newline at end of file diff --git a/src/utils/obb.cpp b/src/utils/obb.cpp deleted file mode 100644 index 13653d4..0000000 --- a/src/utils/obb.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include - -#include - -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -#include -typedef fcl::Vec3f FCL_Vec3; -typedef fcl::OBB FCL_OBB; -#else -#include -typedef fcl::Vector3d FCL_Vec3; -typedef fcl::OBB FCL_OBB; -#endif - -namespace bodies { -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -inline FCL_Vec3 toFcl(const Eigen::Vector3d &eigenVec) { - FCL_Vec3 result; - Eigen::Map(result.data.vs, 3, 1) = eigenVec; - return result; -} -#else -#define toFcl -#endif - -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 -Eigen::Vector3d fromFcl(const FCL_Vec3 &fclVec) { - return Eigen::Map(fclVec.data.vs, 3, 1); -} -#else -#define fromFcl -#endif - -class OBBPrivate : public FCL_OBB { -public: - using FCL_OBB::OBB; -}; - -OBB::OBB() { - obb_.reset(new OBBPrivate); - // Initialize the OBB to position 0, with 0 extents and identity rotation -#if FCL_MAJOR_VERSION > 0 || FCL_MINOR_VERSION > 5 - // FCL 0.6+ does not zero-initialize the OBB. - obb_->extent.setZero(); - obb_->To.setZero(); - obb_->axis.setIdentity(); -#else - // FCL 0.5 zero-initializes the OBB, so we just put the identity into - // orientation. - obb_->axis[0][0] = 1.0; - obb_->axis[1][1] = 1.0; - obb_->axis[2][2] = 1.0; -#endif -} - -OBB::OBB(const OBB &other) { obb_.reset(new OBBPrivate(*other.obb_)); } - -OBB::OBB(const Eigen::Isometry3d &pose, const Eigen::Vector3d &extents) { - obb_.reset(new OBBPrivate); - setPoseAndExtents(pose, extents); -} - -OBB &OBB::operator=(const OBB &other) { - *obb_ = *other.obb_; - return *this; -} - -void OBB::setPoseAndExtents(const Eigen::Isometry3d &pose, - const Eigen::Vector3d &extents) { - const auto rotation = pose.linear(); - -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 - obb_->axis[0] = toFcl(rotation.col(0)); - obb_->axis[1] = toFcl(rotation.col(1)); - obb_->axis[2] = toFcl(rotation.col(2)); -#else - obb_->axis = rotation; -#endif - - obb_->To = toFcl(pose.translation()); - - obb_->extent = {extents[0] / 2.0, extents[1] / 2.0, extents[2] / 2.0}; -} - -void OBB::getExtents(Eigen::Vector3d &extents) const { - extents = 2 * fromFcl(obb_->extent); -} - -Eigen::Vector3d OBB::getExtents() const { - Eigen::Vector3d extents; - getExtents(extents); - return extents; -} - -void OBB::getPose(Eigen::Isometry3d &pose) const { - pose = Eigen::Isometry3d::Identity(); - pose.translation() = fromFcl(obb_->To); -#if FCL_MAJOR_VERSION == 0 && FCL_MINOR_VERSION == 5 - pose.linear().col(0) = fromFcl(obb_->axis[0]); - pose.linear().col(1) = fromFcl(obb_->axis[1]); - pose.linear().col(2) = fromFcl(obb_->axis[2]); -#else - pose.linear() = obb_->axis; -#endif -} - -Eigen::Isometry3d OBB::getPose() const { - Eigen::Isometry3d pose; - getPose(pose); - return pose; -} - -AABB OBB::toAABB() const { - AABB result; - toAABB(result); - return result; -} - -void OBB::toAABB(AABB &aabb) const { - aabb.extendWithTransformedBox(getPose(), getExtents()); -} - -OBB *OBB::extendApprox(const OBB &box) { - if (this->getExtents() == Eigen::Vector3d::Zero()) { - *obb_ = *box.obb_; - return this; - } - - if (this->contains(box)) - return this; - - if (box.contains(*this)) { - *obb_ = *box.obb_; - return this; - } - - *this->obb_ += *box.obb_; - return this; -} - -bool OBB::contains(const Eigen::Vector3d &point) const { - return obb_->contain(toFcl(point)); -} - -bool OBB::overlaps(const bodies::OBB &other) const { - return obb_->overlap(*other.obb_); -} - -EigenSTL::vector_Vector3d OBB::computeVertices() const { - const Eigen::Vector3d e = - getExtents() / 2; // do not use auto type, Eigen would be inefficient - // clang-format off - EigenSTL::vector_Vector3d result = { - { -e[0], -e[1], -e[2] }, - { -e[0], -e[1], e[2] }, - { -e[0], e[1], -e[2] }, - { -e[0], e[1], e[2] }, - { e[0], -e[1], -e[2] }, - { e[0], -e[1], e[2] }, - { e[0], e[1], -e[2] }, - { e[0], e[1], e[2] }, - }; - // clang-format on - - const auto pose = getPose(); - for (auto &v : result) { - v = pose * v; - } - - return result; -} - -bool OBB::contains(const OBB &obb) const { - for (const auto &v : obb.computeVertices()) { - if (!contains(v)) - return false; - } - return true; -} - -OBB::~OBB() = default; - -} // namespace bodies \ No newline at end of file From f286770502b749a9cb1a8a05d3b37404eaa9565b Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 23 Apr 2024 14:08:55 -0400 Subject: [PATCH 36/54] cmake tests --- CMakeLists.txt | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bcdaa15..a97d62d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ else() add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros urdf visualization_msgs GLUT) +set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen urdf visualization_msgs GLUT ) set(MESSAGE_DEPS geometry_msgs std_msgs) find_package(ament_cmake REQUIRED) @@ -28,7 +28,7 @@ find_package(PkgConfig REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME}_msgs "msg/OrientedBoundingBox.msg" "msg/OrientedBoundingBoxStamped.msg" DEPENDENCIES ${MESSAGE_DEPS} LIBRARY_NAME ${PROJECT_NAME}) -include_directories(include) +include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/utils) set(UTILS_SRCS src/utils/bodies.cpp @@ -42,9 +42,11 @@ set(UTILS_SRCS add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME}_utils ${THIS_PACKAGE_DEPS}) +ament_export_libraries(${PROJECT_NAME}_utils) add_library(tf2_sensor_msgs_rbf src/utils/tf2_sensor_msgs.cpp) -target_link_libraries(tf2_sensor_msgs_rbf PRIVATE ${PROJECT_NAME}_utils) +target_link_libraries(tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils) +ament_export_libraries(tf2_sensor_msgs_rbf) add_library(RayCastingShapeMask src/RayCastingShapeMask.cpp) target_link_libraries(RayCastingShapeMask ${PROJECT_NAME}_utils) @@ -53,6 +55,9 @@ add_library(TFFramesWatchdog src/TFFramesWatchdog.cpp) target_link_libraries(TFFramesWatchdog ${PROJECT_NAME}_utils) add_library(${PROJECT_NAME} src/RobotBodyFilter.cpp) +target_include_directories(${PROJECT_NAME} PUBLIC +$ +$) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_utils tf2_sensor_msgs_rbf @@ -64,7 +69,7 @@ ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include) -install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask ${PROJECT_NAME}_utils +install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -133,4 +138,6 @@ if (BUILD_TESTING) endif() ament_export_dependencies(${THIS_PACKAGE_DEPS}) +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils) ament_package() From fd09c319c132dbf94fe305ef666bd4fb150d9d5c Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 23 Apr 2024 18:48:00 -0400 Subject: [PATCH 37/54] include fix --- CMakeLists.txt | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a97d62d..f873a0c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -32,12 +32,12 @@ include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/util set(UTILS_SRCS src/utils/bodies.cpp - # src/utils/cloud.cpp + src/utils/cloud.cpp src/utils/shapes.cpp -# src/utils/string_utils.cpp + src/utils/string_utils.cpp src/utils/tf2_eigen.cpp - # src/utils/tf2_sensor_msgs.cpp - # src/utils/time_utils.cpp + src/utils/tf2_sensor_msgs.cpp + src/utils/time_utils.cpp ) add_library(${PROJECT_NAME}_utils ${UTILS_SRCS}) target_link_libraries(${PROJECT_NAME}_utils ${PCL_LIBRARIES}) @@ -66,10 +66,11 @@ target_link_libraries(${PROJECT_NAME} ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_DEPS}) -install(DIRECTORY include/${PROJECT_NAME}/ +install(DIRECTORY include/ DESTINATION include) install(TARGETS ${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils + EXPORT export_${PROJECT_NAME} RUNTIME DESTINATION bin ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -137,7 +138,11 @@ if (BUILD_TESTING) DESTINATION share/robot_body_filter/test) endif() +ament_export_include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/utils) ament_export_dependencies(${THIS_PACKAGE_DEPS}) ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils) +ament_export_targets( + export_${PROJECT_NAME} +) ament_package() From 35a07e619720f9d9f50ae400f229960f4962626f Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 23 Apr 2024 18:49:13 -0400 Subject: [PATCH 38/54] missing sources --- .../robot_body_filter/utils/tf2_sensor_msgs.h | 2 +- src/RobotBodyFilter.cpp | 2 +- src/utils/cloud.cpp | 2 +- src/utils/string_utils.cpp | 6 ++-- src/utils/tf2_sensor_msgs.cpp | 5 ++- src/utils/time_utils.cpp | 36 +++++++++---------- 6 files changed, 28 insertions(+), 25 deletions(-) diff --git a/include/robot_body_filter/utils/tf2_sensor_msgs.h b/include/robot_body_filter/utils/tf2_sensor_msgs.h index 11865d2..e13fe76 100644 --- a/include/robot_body_filter/utils/tf2_sensor_msgs.h +++ b/include/robot_body_filter/utils/tf2_sensor_msgs.h @@ -3,7 +3,7 @@ #include #include -#include +#include namespace robot_body_filter { diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 2b60297..8a0f420 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include diff --git a/src/utils/cloud.cpp b/src/utils/cloud.cpp index 39e9dbb..802a73d 100644 --- a/src/utils/cloud.cpp +++ b/src/utils/cloud.cpp @@ -2,7 +2,7 @@ #define private protected #include #undef private - +#include #include namespace robot_body_filter diff --git a/src/utils/string_utils.cpp b/src/utils/string_utils.cpp index 5b51f45..e1a849c 100644 --- a/src/utils/string_utils.cpp +++ b/src/utils/string_utils.cpp @@ -1,13 +1,13 @@ #include "robot_body_filter/utils/string_utils.hpp" -#include -#include +// #include +// #include namespace robot_body_filter { void warnLeadingSlash(const std::string& s) { - ROS_WARN_STREAM_ONCE("Found initial slash in " << s); + // ROS_WARN_STREAM_ONCE("Found initial slash in " << s); } void stripLeadingSlash(std::string &s, const bool warn) diff --git a/src/utils/tf2_sensor_msgs.cpp b/src/utils/tf2_sensor_msgs.cpp index 6c887a1..def8341 100644 --- a/src/utils/tf2_sensor_msgs.cpp +++ b/src/utils/tf2_sensor_msgs.cpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include // needs to be implementation-private as we want -march=native optimizations @@ -71,6 +71,9 @@ void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs: *z_out = point.z(); } break; + case CloudChannelType::SCALAR: + //TODO: ADD WARNING FOR NOT SUPPORTED + break; } } diff --git a/src/utils/time_utils.cpp b/src/utils/time_utils.cpp index 5b562b8..e99c55c 100644 --- a/src/utils/time_utils.cpp +++ b/src/utils/time_utils.cpp @@ -2,30 +2,30 @@ namespace robot_body_filter { -ros::Duration remainingTime(const ros::Time &query, const double timeout) +rclcpp::Duration remainingTime(const rclcpp::Time &query, const double timeout) { - ros::Time::waitForValid(ros::WallDuration().fromSec(timeout)); - if (!ros::Time::isValid()) { - ROS_ERROR("ROS time is not yet initialized"); - return ros::Duration(0); - } + // rclcpp::Time::waitForValid(rclcpp::WallDuration().fromSec(timeout)); + // if (!rclcpp::Time::isValid()) { + // ROS_ERROR("ROS time is not yet initialized"); + // return rclcpp::Duration(0); + // } - const auto passed = (ros::Time::now() - query).toSec(); - return ros::Duration(std::max(0.0, timeout - passed)); + // const auto passed = (rclcpp::Time::now() - query).toSec(); + // return rclcpp::Duration(std::max(0.0, timeout - passed)); } -ros::Duration remainingTime(const ros::Time &query, - const ros::Duration &timeout) +rclcpp::Duration remainingTime(const rclcpp::Time &query, + const rclcpp::Duration &timeout) { - ros::Time::waitForValid(ros::WallDuration(timeout.sec, timeout.nsec)); - if (!ros::Time::isValid()) { - ROS_ERROR("ROS time is not yet initialized"); - return ros::Duration(0); - } + // rclcpp::Time::waitForValid(rclcpp::WallDuration(timeout.sec, timeout.nsec)); + // if (!rclcpp::Time::isValid()) { + // // ROS_ERROR("ROS time is not yet initialized"); + // return rclcpp::Duration(0); + // } - const auto passed = ros::Time::now() - query; - const auto remaining = timeout - passed; - return (remaining.sec >= 0) ? remaining : ros::Duration(0); + // const auto passed = rclcpp::Time::now() - query; + // const auto remaining = timeout - passed; + // return (remaining.sec >= 0) ? remaining : rclcpp::Duration(0); } }; \ No newline at end of file From 787487fbb3d86ae3b7ecf88a4cb7f9dda9309bc4 Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 25 Apr 2024 19:05:36 -0400 Subject: [PATCH 39/54] Mostly functional --- examples/body_filter_node.launch.py | 57 +++++ include/robot_body_filter/RobotBodyFilter.h | 7 +- .../robot_body_filter/utils/string_utils.hpp | 5 +- .../robot_body_filter/utils/time_utils.hpp | 5 +- src/RobotBodyFilter.cpp | 206 +++++++++++------- src/TFFramesWatchdog.cpp | 5 +- src/utils/time_utils.cpp | 19 +- 7 files changed, 212 insertions(+), 92 deletions(-) create mode 100644 examples/body_filter_node.launch.py diff --git a/examples/body_filter_node.launch.py b/examples/body_filter_node.launch.py new file mode 100644 index 0000000..4e7ad5a --- /dev/null +++ b/examples/body_filter_node.launch.py @@ -0,0 +1,57 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +def generate_launch_description(): + ld = LaunchDescription() + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("body_filter_node"), + "examples", + "full_example.urdf", + ] + ), + " sim_mode:=", + LaunchConfiguration("sim_mode"), + ] + ) + config = os.path.join( + get_package_share_directory('ros2_tutorials'), + 'config', + 'params.yaml' + ) + + node = Node( + package="ros2_tutorials", + name="your_amazing_node", + executable="test_yaml_params", + parameters=[config], + ) + robot_description = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[ + {"robot_description": robot_description_content}, + ], + ) + ld.add_action(robot_description) + ld.add_action(node) + return ld diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index c6a326e..a2fc266 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -100,9 +100,10 @@ class RobotBodyFilter : public filters::FilterBase { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - RobotBodyFilter(); + RobotBodyFilter(std::shared_ptr inputNode = std::make_shared("robot_body_filter")); ~RobotBodyFilter() override; + void DeclareParameters(); //! Read config parameters loaded by FilterBase::configure(string, NodeHandle) //! Parameters are described in the readme. bool configure() override; @@ -322,7 +323,7 @@ class RobotBodyFilter : public filters::FilterBase { //! tf client std::shared_ptr tfBuffer; //! tf listener - std::unique_ptr tfListener; + std::shared_ptr tfListener; //! Watchdog for unreachable frames. std::shared_ptr tfFramesWatchdog; @@ -462,6 +463,7 @@ class RobotBodyFilter : public filters::FilterBase { class RobotBodyFilterLaserScan : public RobotBodyFilter { public: + void DeclareParameters(); //! Apply the filter. bool update(const sensor_msgs::msg::LaserScan& inputScan, sensor_msgs::msg::LaserScan& filteredScan) override; @@ -477,6 +479,7 @@ class RobotBodyFilterLaserScan : public RobotBodyFilter { public: + void DeclareParameters(); //! Apply the filter. bool update(const sensor_msgs::msg::PointCloud2& inputCloud, sensor_msgs::msg::PointCloud2& filteredCloud) override; diff --git a/include/robot_body_filter/utils/string_utils.hpp b/include/robot_body_filter/utils/string_utils.hpp index 2001f55..fe5b3b6 100644 --- a/include/robot_body_filter/utils/string_utils.hpp +++ b/include/robot_body_filter/utils/string_utils.hpp @@ -126,7 +126,7 @@ inline std::string to_string(const std::vector &value) } template -inline std::string to_string(const std::set &value) +inline std::string to_string(std::set &value) { std::stringstream ss; ss << "["; @@ -142,7 +142,8 @@ inline std::string to_string(const std::set &value) ++i; } ss << "]"; - return ss.str(); + auto string = ss.str(); + return string; } template diff --git a/include/robot_body_filter/utils/time_utils.hpp b/include/robot_body_filter/utils/time_utils.hpp index b6f20f5..14c76dc 100644 --- a/include/robot_body_filter/utils/time_utils.hpp +++ b/include/robot_body_filter/utils/time_utils.hpp @@ -12,7 +12,7 @@ namespace robot_body_filter { * @return The remaining time, or zero duration if the time is negative or ROS * time isn't initialized. */ -rclcpp::Duration remainingTime(const rclcpp::Time &query, double timeout); +rclcpp::Duration remainingTime(rclcpp::Clock clock, const rclcpp::Time &query, const double timeout); /** * @brief remainingTime Return remaining time to timeout from the query time. @@ -21,8 +21,7 @@ rclcpp::Duration remainingTime(const rclcpp::Time &query, double timeout); * @return The remaining time, or zero duration if the time is negative or ROS * time isn't initialized. */ -rclcpp::Duration remainingTime(const rclcpp::Time &query, - const rclcpp::Duration &timeout); +rclcpp::Duration remainingTime(rclcpp::Clock clock, const rclcpp::Time &query, const rclcpp::Duration &timeout); }; diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 8a0f420..3b4087e 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -37,16 +37,24 @@ namespace robot_body_filter { template -RobotBodyFilter::RobotBodyFilter() - : privateNodeHandle("~"), - nodeHandle(std::make_shared("robot_body_filter")), +RobotBodyFilter::RobotBodyFilter(std::shared_ptr inputNode) + : privateNodeHandle("robot_body_filter_private"), + nodeHandle(inputNode), modelPoseUpdateInterval(0, 0), reachableTransformTimeout(0, 0), unreachableTransformTimeout(0, 0), tfBufferLength(0, 0) { - this->modelMutex.reset(new std::mutex()); + this->modelMutex = std::make_shared(); +} + +void RobotBodyFilterPointCloud2::DeclareParameters(){ + this->nodeHandle->declare_parameter("sensor/point_by_point", false); + RobotBodyFilter::DeclareParameters(); +}; - // Declare ROS2 Parameters in node constructor +template +void RobotBodyFilter::DeclareParameters(){ +// Declare ROS2 Parameters in node constructor auto param_desc = rcl_interfaces::msg::ParameterDescriptor{}; param_desc.description = "frames/fixed"; @@ -54,13 +62,13 @@ RobotBodyFilter::RobotBodyFilter() param_desc.description = "frames/sensor"; this->nodeHandle->declare_parameter("sensorFrame", "", param_desc); param_desc.description = "frames/filtering"; - this->nodeHandle->declare_parameter("filteringFrame", this->fixedFrame, param_desc); + this->nodeHandle->declare_parameter("filteringFrame", "base_link", param_desc); param_desc.description = "m"; param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; this->nodeHandle->declare_parameter("minDistance", 0.0, param_desc); this->nodeHandle->declare_parameter("maxDistance", 0.0, param_desc); param_desc.description = "body_model/robot_description_param name"; - this->nodeHandle->declare_parameter("body_model/robot_description_param", "", param_desc); + this->nodeHandle->declare_parameter("body_model/robot_description_param", "robot_description", param_desc); this->nodeHandle->declare_parameter("filter/keep_clouds_organized", true); param_desc.description = "s"; this->nodeHandle->declare_parameter("filter/model_pose_update_interval", 0.0, param_desc); @@ -118,29 +126,39 @@ RobotBodyFilter::RobotBodyFilter() this->nodeHandle->declare_parameters("body_model/inflation/per_link/scale", std::map()); - // Note: some of these are init to empty strings, originally they are vectors - // of strings - this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", ""); - this->nodeHandle->declare_parameter("ignored_links/bounding_box", ""); - this->nodeHandle->declare_parameter("ignored_links/contains_test", ""); - this->nodeHandle->declare_parameter("ignored_links/shadow_test", "laser"); - this->nodeHandle->declare_parameter("ignored_links/everywhere", ""); - this->nodeHandle->declare_parameter("only_links", ""); + this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", std::vector{""}); + this->nodeHandle->declare_parameter("ignored_links/bounding_box", std::vector{""}); + this->nodeHandle->declare_parameter("ignored_links/contains_test", std::vector{""}); + this->nodeHandle->declare_parameter("ignored_links/shadow_test", std::vector{"laser"}); + this->nodeHandle->declare_parameter("ignored_links/everywhere", std::vector{""}); + this->nodeHandle->declare_parameter("only_links", std::vector{""}); this->nodeHandle->declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model"); - this->nodeHandle->declare_parameter("frames/output", this->filteringFrame); + this->nodeHandle->declare_parameter("frames/output", "base_link"); this->nodeHandle->declare_parameter("cloud/point_channels", std::vector{"vp_"}); this->nodeHandle->declare_parameter("cloud/direction_channels", std::vector{"normal_"}); + this->nodeHandle->declare_parameter("transforms/buffer_length", 60.0); + + //TESTING + this->nodeHandle->declare_parameter("robot_description", std::string{""}); } template bool RobotBodyFilter::configure() { - // this->tfBufferLength = this->getParamVerbose("transforms/buffer_length", - // rclcpp::Duration(60.0), "s"); + double tempBufferLength = 0.0; + this->nodeHandle->get_parameter("transforms/buffer_length", tempBufferLength); + this->tfBufferLength = rclcpp::Duration::from_seconds(tempBufferLength); if (this->tfBuffer == nullptr) { tf2::Duration tf2_duration = tf2_ros::fromRclcpp(this->tfBufferLength); - this->tfBuffer = std::make_shared(nodeHandle->get_clock(), tf2_duration); - this->tfListener = std::make_unique(*this->tfBuffer); + RCLCPP_INFO(this->nodeHandle->get_logger(), "Creating TF buffer with length %f", this->tfBufferLength.seconds()); + this->tfBuffer = std::make_shared(this->nodeHandle->get_clock()); + this->tfListener = std::make_shared(*this->tfBuffer); + if(this->tfBuffer->canTransform("base_link", "laser", tf2::TimePointZero)){ + RCLCPP_INFO(this->nodeHandle->get_logger(), "Transform is available INSIDE"); + } + else{ + RCLCPP_INFO(this->nodeHandle->get_logger(), "Transform is not available INSIDE"); + } } else { // clear the TF buffer (useful if calling configure() after receiving old TF // data) @@ -148,10 +166,13 @@ bool RobotBodyFilter::configure() { } this->nodeHandle->get_parameter("fixedFrame", this->fixedFrame); + RCLCPP_DEBUG(this->nodeHandle->get_logger(), "Fixed frame: %s", this->fixedFrame.c_str()); stripLeadingSlash(this->fixedFrame, true); this->nodeHandle->get_parameter("sensorFrame", this->sensorFrame); + RCLCPP_DEBUG(this->nodeHandle->get_logger(), "sensor frame: %s", this->sensorFrame.c_str()); stripLeadingSlash(this->sensorFrame, true); this->nodeHandle->get_parameter("filteringFrame", this->filteringFrame); + RCLCPP_DEBUG(this->nodeHandle->get_logger(), "filtering frame: %s", this->filteringFrame.c_str()); stripLeadingSlash(this->sensorFrame, true); this->nodeHandle->get_parameter("minDistance", this->minDistance); this->nodeHandle->get_parameter("maxDistance", this->maxDistance); @@ -382,7 +403,7 @@ bool RobotBodyFilter::configure() { this->nodeHandle->get_parameter("sensor/point_by_point", this->pointByPointScan); - + //TODO: Update to true ROS2 parameter callback on robot_description // subscribe for robot_description param changes this->robotDescriptionUpdatesListener = this->nodeHandle->template create_subscription( "dynamic_robot_model_server/parameter_updates", 10, @@ -393,17 +414,13 @@ bool RobotBodyFilter::configure() { - // if (this->computeBoundingSphere) { - // this->boundingSpherePublisher = - // this->nodeHandle->template advertise( - // "robot_bounding_sphere", 100); - // } + if (this->computeBoundingSphere) { + this->boundingSpherePublisher = nodeHandle->create_publisher("robot_bounding_sphere", rclcpp::QoS(100)); + } - // if (this->computeBoundingBox) { - // this->boundingBoxPublisher = this->nodeHandle->template - // advertise("robot_bounding_box", - // 100); - // } + if (this->computeBoundingBox) { + this->boundingBoxPublisher = nodeHandle->create_publisher("robot_bounding_box", rclcpp::QoS(100)); + } // if (this->computeOrientedBoundingBox) { // this->orientedBoundingBoxPublisher = this->nodeHandle->template @@ -568,6 +585,8 @@ bool RobotBodyFilter::configure() { initialMonitoredFrames.insert(this->sensorFrame); } auto loop_rate = std::make_shared(rclcpp::Duration::from_seconds(1.0).nanoseconds()); + RCLCPP_DEBUG(this->nodeHandle->get_logger(), "Creating TF frames watchdog"); + RCLCPP_DEBUG(this->nodeHandle->get_logger(), "Filtering data in frame %s", this->filteringFrame.c_str()); this->tfFramesWatchdog = std::make_shared(this->nodeHandle, this->filteringFrame, initialMonitoredFrames, this->tfBuffer, this->unreachableTransformTimeout, loop_rate); @@ -575,9 +594,9 @@ bool RobotBodyFilter::configure() { } { // initialize the robot body to be masked out - + auto sleeper = rclcpp::Rate(rclcpp::Duration::from_seconds(1.0).nanoseconds()); std::string robotUrdf; - while (!this->nodeHandle->get_parameter(this->robotDescriptionParam, robotUrdf) || robotUrdf.length() == 0) { + while (!this->nodeHandle->get_parameter(this->robotDescriptionParam.c_str(), robotUrdf) || robotUrdf.length() == 0) { if (this->failWithoutRobotDescription) { throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); } @@ -612,29 +631,54 @@ bool RobotBodyFilter::configure() { RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to all links except %s.", to_string(this->linksIgnoredEverywhere).c_str()); } - } else { + } + else { if (this->linksIgnoredEverywhere.empty()) { RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); - } else { - RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", - to_string(this->onlyLinks).c_str(), - to_string(this->linksIgnoredEverywhere).c_str()); + } + else { + // this->onlyLinks.insert("test"); + auto value2 = this->onlyLinks.find("test"); + if (value2 != this->onlyLinks.end()) { + RCLCPP_INFO(nodeHandle->get_logger(),"Did we insert element? : %s.", "yes"); + } + RCLCPP_INFO(nodeHandle->get_logger(),"Did we insert element? : %s.", "no"); + std::stringstream ss; + ss << "["; + size_t i = 0; + for (const auto& v : this->onlyLinks) { + if (std::is_same::value) + ss << "\"" << to_string(v) << "\""; + else + ss << to_string(v); + if (i + 1 < this->onlyLinks.size()) ss << ", "; + ++i; + } + ss << "]"; + RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", "test", ss.str().c_str()); + auto localOnlyLinks = this->onlyLinks; + // auto onlyLinks = to_string(localOnlyLinks).c_str(); + // to_string(this->onlyLinks).c_str()); + // to_string(this->linksIgnoredEverywhere).c_str()); } } this->timeConfigured = this->nodeHandle->now(); + //Bad idea? + this->configured_ = true; + return true; } bool RobotBodyFilterLaserScan::configure() { - this->nodeHandle->declare_parameter("sensor/point_by_point", true); + // this->nodeHandle->declare_parameter("sensor/point_by_point", true); bool success = RobotBodyFilter::configure(); return false; } bool RobotBodyFilterPointCloud2::configure() { - this->nodeHandle->declare_parameter("sensor/point_by_point", false); + RCLCPP_INFO(nodeHandle->get_logger(),"Configuring"); bool success = RobotBodyFilter::configure(); if (!success) return false; @@ -667,20 +711,24 @@ bool RobotBodyFilter::computeMask( // compute a mask of point indices for points from projectedPointCloud // that tells if they are inside or outside robot, or shadow points - + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); if (!this->pointByPointScan) { Eigen::Vector3d sensorPosition; try { + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); const auto sensorTf = this->tfBuffer->lookupTransform(this->filteringFrame, sensorFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout)); + remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout)); + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); tf2::fromMsg(sensorTf.transform.translation, sensorPosition); } catch (tf2::TransformException& e) { + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); RCLCPP_ERROR(nodeHandle->get_logger(), "RobotBodyFilter: Could not compute filtering mask due to this " "TF exception: %s", e.what()); return false; } + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); // update transforms cache, which is then used in body masking this->updateTransformCache(scanTime); @@ -698,6 +746,7 @@ bool RobotBodyFilter::computeMask( CloudConstIter stamps_it(projectedPointCloud, "stamps"); pointMask.resize(num_points(projectedPointCloud)); + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); double scanDuration = 0.0; for (CloudConstIter stamps_end_it(projectedPointCloud, "stamps"); stamps_end_it != stamps_end_it.end(); @@ -716,6 +765,7 @@ bool RobotBodyFilter::computeMask( // prevent division by zero if (updateBodyPosesEvery == 0) updateBodyPosesEvery = 1; } + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); // prevent division by zero in ratio computation in case the pointcloud // isn't really taken point by point with different timestamps @@ -733,6 +783,7 @@ bool RobotBodyFilter::computeMask( Eigen::Vector3f point; Eigen::Vector3d viewPoint; RayCastingShapeMask::MaskValue mask; + RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); this->cacheLookupBetweenScansRatio = 0.0; for (size_t i = 0; i < num_points(projectedPointCloud); @@ -775,6 +826,9 @@ bool RobotBodyFilter::computeMask( bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputScan, sensor_msgs::msg::LaserScan& filteredScan) { + RCLCPP_DEBUG(nodeHandle->get_logger(), "UPDATING MEMERS"); + + const auto& headerScanTime = inputScan.header.stamp; const auto& scanTime = rclcpp::Time(inputScan.header.stamp); @@ -855,9 +909,9 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc std::string err; if (!this->tfBuffer->canTransform(this->fixedFrame, scanFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err) || + remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err) || !this->tfBuffer->canTransform(this->fixedFrame, scanFrame, afterScanTime, - remainingTime(afterScanTime, this->reachableTransformTimeout), &err)) { + remainingTime(*this->nodeHandle->get_clock(), afterScanTime, this->reachableTransformTimeout), &err)) { if (err.find("future") != std::string::npos) { const auto delay = nodeHandle->now() - scanTime; auto& clk = *nodeHandle->get_clock(); @@ -909,7 +963,7 @@ bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputSc tmpPointCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); std::string err; if (!this->tfBuffer->canTransform(this->filteringFrame, tmpPointCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { auto& clk = *nodeHandle->get_clock(); RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform " @@ -993,10 +1047,14 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp return false; } + RCLCPP_ERROR(nodeHandle->get_logger(), "Passed checks"); + const auto inputCloudFrame = this->sensorFrame.empty() ? stripLeadingSlash(inputCloud.header.frame_id, true) : this->sensorFrame; if (!this->tfFramesWatchdog->isReachable(inputCloudFrame)) { + RCLCPP_DEBUG(nodeHandle->get_logger(), + "inputCloudFrame: %s", inputCloudFrame.c_str()); RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Throwing away scan since sensor frame is " "unreachable."); @@ -1057,6 +1115,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } // Transform to filtering frame + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM"); sensor_msgs::msg::PointCloud2 transformedCloud; if (inputCloud.header.frame_id == this->filteringFrame) { @@ -1066,8 +1125,8 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform(this->filteringFrame, inputCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + if (!this->tfBuffer->canTransform("base_link", "base_link", scanTime, + remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { auto& clk = *nodeHandle->get_clock(); RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform " @@ -1080,6 +1139,8 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp this->channelsToTransform); } + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM2"); + // Compute the mask and use it (transform message only if sensorFrame is // specified) std::vector pointMask; @@ -1091,6 +1152,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } // Filter the cloud + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM3"); sensor_msgs::msg::PointCloud2 tmpCloud; CREATE_FILTERED_CLOUD(transformedCloud, tmpCloud, this->keepCloudsOrganized, @@ -1105,8 +1167,8 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp tmpCloud.header.frame_id.c_str(), this->outputFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform(this->outputFrame, tmpCloud.header.frame_id, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + if (!this->tfBuffer->canTransform("laser", "base_link", scanTime, + remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { auto& clk = *nodeHandle->get_clock(); RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform " @@ -1118,6 +1180,8 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, this->outputFrame, this->channelsToTransform); } + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM4"); + return true; } @@ -1189,7 +1253,7 @@ void RobotBodyFilter::updateTransformCache(const rclcpp::Time& time, const rc { auto linkTransformTfOptional = this->tfFramesWatchdog->lookupTransform( - linkFrame, time, remainingTime(time, this->reachableTransformTimeout)); + linkFrame, time, remainingTime(*this->nodeHandle->get_clock(), time, this->reachableTransformTimeout)); if (!linkTransformTfOptional) // has no value continue; @@ -1205,7 +1269,7 @@ void RobotBodyFilter::updateTransformCache(const rclcpp::Time& time, const rc if (afterScanTime.seconds() != 0) { auto linkTransformTfOptional = this->tfFramesWatchdog->lookupTransform( - linkFrame, afterScanTime, remainingTime(time, this->reachableTransformTimeout)); + linkFrame, afterScanTime, remainingTime(*this->nodeHandle->get_clock(), time, this->reachableTransformTimeout)); if (!linkTransformTfOptional) // has no value continue; @@ -1234,16 +1298,12 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { // parse the URDF model urdf::Model parsedUrdfModel; bool urdfParseSucceeded = parsedUrdfModel.initString(urdfModel); - // if (!urdfParseSucceeded) { - // RCLCPP_ERROR(nodeHandle->get_logger(),"RobotBodyFilter: The URDF model - // given in parameter '" - // << this->robotDescriptionParam - // << "' cannot be parsed. See " - // "urdf::Model::initString for debugging, or try - // running " - // "'gzsdf my_robot.urdf'"); - // return; - // } + if (!urdfParseSucceeded) { + RCLCPP_ERROR(nodeHandle->get_logger(), + "RobotBodyFilter: The URDF model given in parameter %s cannot be parsed. See urdf::Model::initString for debugging, or try running gzsdf my_robot.urdf", + this->robotDescriptionParam.c_str()); + return; + } { std::lock_guard guard(*this->modelMutex); @@ -1822,18 +1882,18 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( const auto& scanTime = projectedPointCloud.header.stamp; std::string err; - try { - if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, - remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), err.c_str()); - return; - } - } catch (tf2::TransformException& e) { - RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), e.what()); - return; - } + // try { + // if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, + // remainingTime(scanTime, this->reachableTransformTimeout), &err)) { + // RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), + // this->localBoundingBoxFrame.c_str(), err.c_str()); + // return; + // } + // } catch (tf2::TransformException& e) { + // RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), + // this->localBoundingBoxFrame.c_str(), e.what()); + // return; + // } const auto localTfMsg = this->tfBuffer->lookupTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime); const Eigen::Isometry3d localTf = tf2::transformToEigen(localTfMsg.transform); diff --git a/src/TFFramesWatchdog.cpp b/src/TFFramesWatchdog.cpp index 17a8cb8..08ca2c6 100644 --- a/src/TFFramesWatchdog.cpp +++ b/src/TFFramesWatchdog.cpp @@ -67,6 +67,7 @@ void TFFramesWatchdog::searchForReachableFrames() break; } std::string err; + RCLCPP_DEBUG(this->nodeHandle->get_logger(), "robot frame: %s, checking against: %s" , this->robotFrame.c_str(), frame.c_str()); if (this->tfBuffer->canTransform(this->robotFrame, frame, time, this->unreachableTfLookupTimeout, &err)) { this->markReachable(frame); RCLCPP_DEBUG(this->nodeHandle->get_logger(),"TFFramesWatchdog (%s): Frame %s became reachable at %i.%i", @@ -136,7 +137,7 @@ optional TFFramesWatchdog::lookupTransform } if (!this->tfBuffer->canTransform(this->robotFrame, frame, time, - remainingTime(time, timeout), errstr)) { + remainingTime(*this->nodeHandle->get_clock(), time, timeout), errstr)) { auto &clk = *nodeHandle->get_clock(); RCLCPP_WARN_THROTTLE(this->nodeHandle->get_logger(), clk, 3, "TFFramesWatchdog (%s): Frame %s became unreachable. Cause: %s", this->robotFrame.c_str(), @@ -149,7 +150,7 @@ optional TFFramesWatchdog::lookupTransform try { return this->tfBuffer->lookupTransform( - this->robotFrame, frame, time, remainingTime(time, timeout)); + this->robotFrame, frame, time, remainingTime(*this->nodeHandle->get_clock(), time, timeout)); } catch (tf2::LookupException&) { auto &clk = *nodeHandle->get_clock(); RCLCPP_WARN_THROTTLE(this->nodeHandle->get_logger(), clk, 3, diff --git a/src/utils/time_utils.cpp b/src/utils/time_utils.cpp index e99c55c..7bf7921 100644 --- a/src/utils/time_utils.cpp +++ b/src/utils/time_utils.cpp @@ -2,30 +2,29 @@ namespace robot_body_filter { -rclcpp::Duration remainingTime(const rclcpp::Time &query, const double timeout) +rclcpp::Duration remainingTime(rclcpp::Clock clock, const rclcpp::Time &query, const double timeout) { - // rclcpp::Time::waitForValid(rclcpp::WallDuration().fromSec(timeout)); + // rclcpp::Time::waitForValid(rclcpp::Duration::from_seconds(timeout)); // if (!rclcpp::Time::isValid()) { // ROS_ERROR("ROS time is not yet initialized"); // return rclcpp::Duration(0); // } - // const auto passed = (rclcpp::Time::now() - query).toSec(); - // return rclcpp::Duration(std::max(0.0, timeout - passed)); + const auto passed = (clock.now() - query).seconds(); + return rclcpp::Duration::from_seconds(std::max(0.0, timeout - passed)); } -rclcpp::Duration remainingTime(const rclcpp::Time &query, - const rclcpp::Duration &timeout) +rclcpp::Duration remainingTime(rclcpp::Clock clock, const rclcpp::Time &query, const rclcpp::Duration &timeout) { - // rclcpp::Time::waitForValid(rclcpp::WallDuration(timeout.sec, timeout.nsec)); + // rclcpp::Time::waitForValid(rclcpp::Duration(timeout.seconds(), timeout.nanoseconds())); // if (!rclcpp::Time::isValid()) { // // ROS_ERROR("ROS time is not yet initialized"); // return rclcpp::Duration(0); // } - // const auto passed = rclcpp::Time::now() - query; - // const auto remaining = timeout - passed; - // return (remaining.sec >= 0) ? remaining : rclcpp::Duration(0); + const auto passed = clock.now() - query; + const auto remaining = timeout - passed; + return (remaining.seconds() >= 0) ? remaining : rclcpp::Duration::from_seconds(0); } }; \ No newline at end of file From c31ede85884d047e6be1b8eef6b60222ce8fed37 Mon Sep 17 00:00:00 2001 From: seb Date: Fri, 26 Apr 2024 14:49:51 -0400 Subject: [PATCH 40/54] runs an entire update loop without getting stuck --- src/RobotBodyFilter.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 3b4087e..fbf0967 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -812,12 +812,12 @@ bool RobotBodyFilter::computeMask( RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); - this->publishDebugPointClouds(projectedPointCloud, pointMask); - this->publishDebugMarkers(scanTime); - this->computeAndPublishBoundingSphere(projectedPointCloud); - this->computeAndPublishBoundingBox(projectedPointCloud); - this->computeAndPublishOrientedBoundingBox(projectedPointCloud); - this->computeAndPublishLocalBoundingBox(projectedPointCloud); + // this->publishDebugPointClouds(projectedPointCloud, pointMask); + // this->publishDebugMarkers(scanTime); + // this->computeAndPublishBoundingSphere(projectedPointCloud); + // this->computeAndPublishBoundingBox(projectedPointCloud); + // this->computeAndPublishOrientedBoundingBox(projectedPointCloud); + // this->computeAndPublishLocalBoundingBox(projectedPointCloud); RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); From 80025318b95d8c53e24164e1cb47a5252ecc7086 Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 29 Apr 2024 18:14:52 -0400 Subject: [PATCH 41/54] working kinda of --- src/RayCastingShapeMask.cpp | 21 +-- src/RobotBodyFilter.cpp | 281 ++++++++++++++++------------------ src/utils/tf2_sensor_msgs.cpp | 15 +- 3 files changed, 158 insertions(+), 159 deletions(-) diff --git a/src/RayCastingShapeMask.cpp b/src/RayCastingShapeMask.cpp index 88ae19a..be0a091 100644 --- a/src/RayCastingShapeMask.cpp +++ b/src/RayCastingShapeMask.cpp @@ -1,5 +1,7 @@ /* HACK HACK HACK */ /* We want to subclass ShapeMask and use its private members. */ +#include +#include #include // has to be there, otherwise we encounter build problems #define private protected #include @@ -161,22 +163,21 @@ void RayCastingShapeMask::updateBodyPosesNoLock() else { if (containsBody == nullptr){ - // ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", - // "Missing transform for shape with handle " << containsHandle - // << " without a body"); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %s without a body", containsHandle); } else { std::string name; if (this->data->shapeNames.find(containsHandle) != this->data->shapeNames.end()) name = this->data->shapeNames.at(containsHandle); - // if (name.empty()) - // ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", - // "Missing transform for shape " << containsBody->getType() - // << " with handle " << containsHandle); - // else - // ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(3, "shape_mask", - // "Missing transform for shape " << name << " (" << containsBody->getType() << ")"); + if (name.empty()) + { + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s with handle %s", containsBody->getType(), containsHandle); + } + else + { + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%s)", name.c_str(), containsBody->getType()); + } } } } diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index fbf0967..184d134 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -62,7 +62,7 @@ void RobotBodyFilter::DeclareParameters(){ param_desc.description = "frames/sensor"; this->nodeHandle->declare_parameter("sensorFrame", "", param_desc); param_desc.description = "frames/filtering"; - this->nodeHandle->declare_parameter("filteringFrame", "base_link", param_desc); + this->nodeHandle->declare_parameter("filteringFrame", rclcpp::ParameterType::PARAMETER_STRING, param_desc); param_desc.description = "m"; param_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; this->nodeHandle->declare_parameter("minDistance", 0.0, param_desc); @@ -76,7 +76,7 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameter("filter/do_contains_test", true); this->nodeHandle->declare_parameter("filter/do_shadow_test", true); param_desc.description = "m"; - this->nodeHandle->declare_parameter("filter/max_shadow_distance", this->maxDistance, param_desc); + this->nodeHandle->declare_parameter("filter/max_shadow_distance", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); param_desc.description = "s"; this->nodeHandle->declare_parameter("transforms/timeout/reachable", 0.1, param_desc); this->nodeHandle->declare_parameter("transforms/timeout/unreachable", 0.2, param_desc); @@ -97,7 +97,7 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameter("bounding_box/marker", false); this->nodeHandle->declare_parameter("oriented_bounding_box/marker", false); this->nodeHandle->declare_parameter("local_bounding_box/marker", false); - this->nodeHandle->declare_parameter("local_bounding_box/frame_id", this->fixedFrame); + this->nodeHandle->declare_parameter("local_bounding_box/frame_id", rclcpp::ParameterType::PARAMETER_STRING); this->nodeHandle->declare_parameter("debug/pcl/inside", false); this->nodeHandle->declare_parameter("debug/pcl/clip", false); this->nodeHandle->declare_parameter("debug/pcl/shadow", false); @@ -171,7 +171,7 @@ bool RobotBodyFilter::configure() { this->nodeHandle->get_parameter("sensorFrame", this->sensorFrame); RCLCPP_DEBUG(this->nodeHandle->get_logger(), "sensor frame: %s", this->sensorFrame.c_str()); stripLeadingSlash(this->sensorFrame, true); - this->nodeHandle->get_parameter("filteringFrame", this->filteringFrame); + this->nodeHandle->get_parameter_or("filteringFrame", this->filteringFrame, this->fixedFrame); RCLCPP_DEBUG(this->nodeHandle->get_logger(), "filtering frame: %s", this->filteringFrame.c_str()); stripLeadingSlash(this->sensorFrame, true); this->nodeHandle->get_parameter("minDistance", this->minDistance); @@ -196,7 +196,7 @@ bool RobotBodyFilter::configure() { const bool doShadowTest = tempDoShadowTest; double tempMaxShadowDistance; - this->nodeHandle->get_parameter("filter/max_shadow_distance", tempMaxShadowDistance); + this->nodeHandle->get_parameter_or("filter/max_shadow_distance", tempMaxShadowDistance, this->maxDistance); const double maxShadowDistance = tempMaxShadowDistance; double tempReachableTransformTimeout; @@ -247,7 +247,7 @@ bool RobotBodyFilter::configure() { this->nodeHandle->get_parameter("local_bounding_box/marker", this->publishLocalBoundingBoxMarker); - this->nodeHandle->get_parameter("local_bounding_box/frame_id", this->localBoundingBoxFrame); + this->nodeHandle->get_parameter_or("local_bounding_box/frame_id", this->localBoundingBoxFrame, this->fixedFrame); this->nodeHandle->get_parameter("debug/pcl/inside", this->publishDebugPclInside); @@ -369,7 +369,7 @@ bool RobotBodyFilter::configure() { // can contain either whole link names, or scoped names of their // collisions (i.e. "link::collision_1" or "link::my_collision") - // Note: ROS2 does not by default have a set parameter, this was the + // Note: ROS2 does not by default have a std::set parameter, this was the // workaround std::vector tempLinksIgnoredInBoundingSphereVector; this->nodeHandle->get_parameter("ignored_links/bounding_sphere", tempLinksIgnoredInBoundingSphereVector); @@ -412,8 +412,6 @@ bool RobotBodyFilter::configure() { // this->reloadRobotModelServiceServer = this->nodeHandle->template create_service( // "reload_model", std::bind(&RobotBodyFilter::triggerModelReload, this, std::placeholders::_1, std::placeholders::_2)); - - if (this->computeBoundingSphere) { this->boundingSpherePublisher = nodeHandle->create_publisher("robot_bounding_sphere", rclcpp::QoS(100)); } @@ -422,150 +420,127 @@ bool RobotBodyFilter::configure() { this->boundingBoxPublisher = nodeHandle->create_publisher("robot_bounding_box", rclcpp::QoS(100)); } - // if (this->computeOrientedBoundingBox) { - // this->orientedBoundingBoxPublisher = this->nodeHandle->template - // advertise("robot_oriented_bounding_box", - // 100); - // } + if (this->computeOrientedBoundingBox) { + this->orientedBoundingBoxPublisher = nodeHandle->create_publisher("robot_oriented_bounding_box", rclcpp::QoS(100)); + } - // if (this->computeLocalBoundingBox) { - // this->localBoundingBoxPublisher = this->nodeHandle->template - // advertise("robot_local_bounding_box", - // 100); - // } + if (this->computeLocalBoundingBox) { + this->localBoundingBoxPublisher = nodeHandle->create_publisher("robot_local_bounding_box", rclcpp::QoS(100)); + } - // if (this->publishBoundingSphereMarker && this->computeBoundingSphere) { - // this->boundingSphereMarkerPublisher = this->nodeHandle->template - // advertise("robot_bounding_sphere_marker", - // 100); - // } + if (this->publishBoundingSphereMarker && this->computeBoundingSphere) { + this->boundingSphereMarkerPublisher = this->nodeHandle->template + create_publisher("robot_bounding_sphere_marker", + 100); + } - // if (this->publishBoundingBoxMarker && this->computeBoundingBox) { - // this->boundingBoxMarkerPublisher = this->nodeHandle->template - // advertise("robot_bounding_box_marker", - // 100); - // } + if (this->publishBoundingBoxMarker && this->computeBoundingBox) { + this->boundingBoxMarkerPublisher = this->nodeHandle->template + create_publisher("robot_bounding_box_marker", + 100); + } - // if (this->publishOrientedBoundingBoxMarker && - // this->computeOrientedBoundingBox) { - // this->orientedBoundingBoxMarkerPublisher = this->nodeHandle->template - // advertise("robot_oriented_bounding_box_marker", - // 100); - // } + if (this->publishOrientedBoundingBoxMarker && this->computeOrientedBoundingBox) { + this->orientedBoundingBoxMarkerPublisher = this->nodeHandle->template + create_publisher("robot_oriented_bounding_box_marker", + 100); + } - // if (this->publishLocalBoundingBoxMarker && - // this->computeLocalBoundingBox) { - // this->localBoundingBoxMarkerPublisher = this->nodeHandle->template - // advertise("robot_local_bounding_box_marker", - // 100); - // } + if (this->publishLocalBoundingBoxMarker && this->computeLocalBoundingBox) { + this->localBoundingBoxMarkerPublisher = this->nodeHandle->template + create_publisher("robot_local_bounding_box_marker", + 100); + } - // if (this->publishNoBoundingBoxPointcloud) - // { - // this->scanPointCloudNoBoundingBoxPublisher = - // this->nodeHandle->template - // advertise("scan_point_cloud_no_bbox", - // 100); - // } + if (this->publishNoBoundingBoxPointcloud) { + this->scanPointCloudNoBoundingBoxPublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_no_bbox", + 100); + } - // if (this->publishNoOrientedBoundingBoxPointcloud) - // { - // this->scanPointCloudNoOrientedBoundingBoxPublisher = - // this->nodeHandle->template - // advertise("scan_point_cloud_no_oriented_bbox", - // 100); - // } + if (this->publishNoOrientedBoundingBoxPointcloud) { + this->scanPointCloudNoOrientedBoundingBoxPublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_no_oriented_bbox", + 100); + } - // if (this->publishNoLocalBoundingBoxPointcloud) - // { - // this->scanPointCloudNoLocalBoundingBoxPublisher = - // this->nodeHandle->template - // advertise("scan_point_cloud_no_local_bbox", - // 100); - // } + if (this->publishNoLocalBoundingBoxPointcloud) { + this->scanPointCloudNoLocalBoundingBoxPublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_no_local_bbox", + 100); + } - // if (this->publishNoBoundingSpherePointcloud) - // { - // this->scanPointCloudNoBoundingSpherePublisher = - // this->nodeHandle->template - // advertise("scan_point_cloud_no_bsphere", - // 100); - // } + if (this->publishNoBoundingSpherePointcloud) { + this->scanPointCloudNoBoundingSpherePublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_no_bsphere", + 100); + } - // if (this->publishDebugPclInside) - // { - // this->debugPointCloudInsidePublisher = this->nodeHandle->template - // advertise("scan_point_cloud_inside", - // 100); - // } + if (this->publishDebugPclInside) { + this->debugPointCloudInsidePublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_inside", + 100); + } - // if (this->publishDebugPclClip) - // { - // this->debugPointCloudClipPublisher = this->nodeHandle->template - // advertise("scan_point_cloud_clip", - // 100); - // } + if (this->publishDebugPclClip) { + this->debugPointCloudClipPublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_clip", + 100); + } - // if (this->publishDebugPclShadow) - // { - // this->debugPointCloudShadowPublisher = this->nodeHandle->template - // advertise("scan_point_cloud_shadow", - // 100); - // } + if (this->publishDebugPclShadow) { + this->debugPointCloudShadowPublisher = this->nodeHandle->template + create_publisher("scan_point_cloud_shadow", + 100); + } - // if (this->publishDebugContainsMarker) - // { - // this->debugContainsMarkerPublisher = this->nodeHandle->template - // advertise("robot_model_for_contains_test", - // 100); - // } + if (this->publishDebugContainsMarker) { + this->debugContainsMarkerPublisher = this->nodeHandle->template + create_publisher("robot_model_for_contains_test", + 100); + } - // if (this->publishDebugShadowMarker) - // { - // this->debugShadowMarkerPublisher = this->nodeHandle->template - // advertise("robot_model_for_shadow_test", - // 100); - // } + if (this->publishDebugShadowMarker) { + this->debugShadowMarkerPublisher = this->nodeHandle->template + create_publisher("robot_model_for_shadow_test", + 100); + } - // if (this->publishDebugBsphereMarker) - // { - // this->debugBsphereMarkerPublisher = this->nodeHandle->template - // advertise("robot_model_for_bounding_sphere", - // 100); - // } + if (this->publishDebugBsphereMarker) { + this->debugBsphereMarkerPublisher = this->nodeHandle->template + create_publisher("robot_model_for_bounding_sphere", + 100); + } - // if (this->publishDebugBboxMarker) - // { - // this->debugBboxMarkerPublisher = this->nodeHandle->template - // advertise("robot_model_for_bounding_box", - // 100); - // } + if (this->publishDebugBboxMarker) { + this->debugBboxMarkerPublisher = this->nodeHandle->template + create_publisher("robot_model_for_bounding_box", + 100); + } - // if (this->computeDebugBoundingBox) { - // this->boundingBoxDebugMarkerPublisher = this->nodeHandle->template - // advertise( - // "robot_bounding_box_debug", 100); - // } + if (this->computeDebugBoundingBox) { + this->boundingBoxDebugMarkerPublisher = this->nodeHandle->template + create_publisher("robot_bounding_box_debug", + 100); + } - // if (this->computeDebugOrientedBoundingBox) { - // this->orientedBoundingBoxDebugMarkerPublisher = - // this->nodeHandle->template - // advertise( - // "robot_oriented_bounding_box_debug", 100); - // } + if (this->computeDebugOrientedBoundingBox) { + this->orientedBoundingBoxDebugMarkerPublisher = this->nodeHandle->template + create_publisher("robot_oriented_bounding_box_debug", + 100); + } - // if (this->computeDebugLocalBoundingBox) { - // this->localBoundingBoxDebugMarkerPublisher = - // this->nodeHandle->template - // advertise( - // "robot_local_bounding_box_debug", 100); - // } + if (this->computeDebugLocalBoundingBox) { + this->localBoundingBoxDebugMarkerPublisher = this->nodeHandle->template + create_publisher("robot_local_bounding_box_debug", + 100); + } - // if (this->computeDebugBoundingSphere) { - // this->boundingSphereDebugMarkerPublisher = this->nodeHandle->template - // advertise( - // "robot_bounding_sphere_debug", 100); - // } + if (this->computeDebugBoundingSphere) { + this->boundingSphereDebugMarkerPublisher = this->nodeHandle->template + create_publisher("robot_bounding_sphere_debug", + 100); + } // initialize the 3D body masking tool auto getShapeTransformCallback = @@ -666,6 +641,7 @@ bool RobotBodyFilter::configure() { this->timeConfigured = this->nodeHandle->now(); //Bad idea? + //This should be handeled by the robotDescriptionUpdate callback this->configured_ = true; return true; @@ -683,7 +659,7 @@ bool RobotBodyFilterPointCloud2::configure() { bool success = RobotBodyFilter::configure(); if (!success) return false; - this->nodeHandle->get_parameter("frames/output", this->outputFrame); + this->nodeHandle->get_parameter_or("frames/output", this->outputFrame, this->filteringFrame); std::vector tempPointChannels; std::vector tempDirectionChannels; this->nodeHandle->get_parameter("cloud/point_channels", tempPointChannels); @@ -812,12 +788,12 @@ bool RobotBodyFilter::computeMask( RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Mask computed in %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); - // this->publishDebugPointClouds(projectedPointCloud, pointMask); - // this->publishDebugMarkers(scanTime); - // this->computeAndPublishBoundingSphere(projectedPointCloud); - // this->computeAndPublishBoundingBox(projectedPointCloud); - // this->computeAndPublishOrientedBoundingBox(projectedPointCloud); - // this->computeAndPublishLocalBoundingBox(projectedPointCloud); + this->publishDebugPointClouds(projectedPointCloud, pointMask); + this->publishDebugMarkers(scanTime); + this->computeAndPublishBoundingSphere(projectedPointCloud); + this->computeAndPublishBoundingBox(projectedPointCloud); + this->computeAndPublishOrientedBoundingBox(projectedPointCloud); + this->computeAndPublishLocalBoundingBox(projectedPointCloud); RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); @@ -1116,25 +1092,33 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp // Transform to filtering frame RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM"); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", + inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); + + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.25"); sensor_msgs::msg::PointCloud2 transformedCloud; if (inputCloud.header.frame_id == this->filteringFrame) { transformedCloud = inputCloud; + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.5"); } else { RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.5"); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform("base_link", "base_link", scanTime, + RCLCPP_INFO(nodeHandle->get_logger(), "remaining time: %f", remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout).seconds()); + if (!this->tfBuffer->canTransform(this->filteringFrame, inputCloud.header.frame_id, scanTime, remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { auto& clk = *nodeHandle->get_clock(); RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, "RobotBodyFilter: Cannot transform " "point cloud to filtering frame. Something's wrong with TFs: %s", err.c_str()); + RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.75"); return false; } - + RCLCPP_ERROR(nodeHandle->get_logger(), "Before Transform"); transformWithChannels(inputCloud, transformedCloud, *this->tfBuffer, this->filteringFrame, this->channelsToTransform); } @@ -1167,7 +1151,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp tmpCloud.header.frame_id.c_str(), this->outputFrame.c_str()); std::lock_guard guard(*this->modelMutex); std::string err; - if (!this->tfBuffer->canTransform("laser", "base_link", scanTime, + if (!this->tfBuffer->canTransform(this->outputFrame, tmpCloud.header.frame_id, scanTime, remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { auto& clk = *nodeHandle->get_clock(); RCLCPP_ERROR_THROTTLE(nodeHandle->get_logger(), clk, 3, @@ -1536,6 +1520,8 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { if (!this->computeBoundingSphere && !this->computeDebugBoundingSphere) return; + RCLCPP_INFO(nodeHandle->get_logger(), "Computing bounding sphere"); + // assume this->modelMutex is locked // when computing bounding spheres for publication, we want to publish them to @@ -1550,24 +1536,29 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( std::vector spheres; { visualization_msgs::msg::MarkerArray boundingSphereDebugMsg; + RCLCPP_INFO(nodeHandle->get_logger(), "Compute Bounding Sphere Debug bool: %d", this->computeDebugBoundingSphere); + for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingSphere()) { const auto& shapeHandle = shapeHandleAndBody.first; const auto& body = shapeHandleAndBody.second; - + RCLCPP_INFO(nodeHandle->get_logger(), "Compute Bounding Sphere Debug bool: %d", this->computeDebugBoundingSphere); if (this->shapesIgnoredInBoundingSphere.find(shapeHandle) != this->shapesIgnoredInBoundingSphere.end()) continue; bodies::BoundingSphere sphere; body->computeBoundingSphere(sphere); spheres.push_back(sphere); - + RCLCPP_INFO(nodeHandle->get_logger(), "Compute Bounding Sphere Debug bool: %d", this->computeDebugBoundingSphere); if (this->computeDebugBoundingSphere) { + RCLCPP_INFO(nodeHandle->get_logger(), "Computing debug sphere"); visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; msg.scale.x = msg.scale.y = msg.scale.z = sphere.radius * 2; + RCLCPP_INFO(nodeHandle->get_logger(), "Sphere center: %f %f %f", sphere.center[0], sphere.center[1], sphere.center[2]); + msg.pose.position.x = sphere.center[0]; msg.pose.position.y = sphere.center[1]; msg.pose.position.z = sphere.center[2]; diff --git a/src/utils/tf2_sensor_msgs.cpp b/src/utils/tf2_sensor_msgs.cpp index def8341..48197dd 100644 --- a/src/utils/tf2_sensor_msgs.cpp +++ b/src/utils/tf2_sensor_msgs.cpp @@ -37,10 +37,10 @@ void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs: { if (num_points(cloudIn) == 0) return; - +RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "after num points"); if (type == CloudChannelType::SCALAR) return; - +RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "after scalar check"); CloudConstIter x_in(cloudIn, channelPrefix + "x"); CloudConstIter y_in(cloudIn, channelPrefix + "y"); CloudConstIter z_in(cloudIn, channelPrefix + "z"); @@ -60,7 +60,9 @@ void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs: *x_out = point.x(); *y_out = point.y(); *z_out = point.z(); + // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "looping"); } + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "break"); break; case CloudChannelType::DIRECTION: for (; x_out != x_out.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) @@ -69,7 +71,10 @@ void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs: *x_out = point.x(); *y_out = point.y(); *z_out = point.z(); + // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "looping"); + } + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "break"); break; case CloudChannelType::SCALAR: //TODO: ADD WARNING FOR NOT SUPPORTED @@ -95,6 +100,7 @@ sensor_msgs::msg::PointCloud2& transformWithChannels( const std::unordered_map& channels) { std::unordered_set channelsPresent; + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "first loop"); for (const auto& field: in.fields) { for (const auto& channelAndType : channels) { @@ -105,15 +111,16 @@ sensor_msgs::msg::PointCloud2& transformWithChannels( } } + out = in; out.header = tf.header; const auto t = tf2::transformToEigen(tf).cast(); - transformChannel(in, out, t, "", CloudChannelType::POINT); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "second loop"); for (const auto& channel : channelsPresent) transformChannel(in, out, t, channel, channels.at(channel)); - + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "before return"); return out; } From d2ea7c793047767a6f647fe3553ffd166b6c33d9 Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 1 May 2024 00:08:37 +0000 Subject: [PATCH 42/54] removed debug msgs, fixed depends, parsing urdf --- CMakeLists.txt | 5 +- include/robot_body_filter/RobotBodyFilter.h | 5 +- package.xml | 3 +- src/RobotBodyFilter.cpp | 176 +++++++++----------- src/utils/tf2_sensor_msgs.cpp | 9 - 5 files changed, 89 insertions(+), 109 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f873a0c..27424ef 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ else() add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -set(THIS_PACKAGE_DEPS filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen urdf visualization_msgs GLUT ) +set(THIS_PACKAGE_DEPS urdf urdf_parser_plugin filters geometric_shapes_local laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen urdf visualization_msgs GLUT ) set(MESSAGE_DEPS geometry_msgs std_msgs) find_package(ament_cmake REQUIRED) @@ -139,8 +139,7 @@ if (BUILD_TESTING) endif() ament_export_include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/utils) -ament_export_dependencies(${THIS_PACKAGE_DEPS}) -ament_export_include_directories(include) +ament_export_dependencies(${THIS_PACKAGE_DEPS} rosidl_default_runtime) ament_export_libraries(${PROJECT_NAME} TFFramesWatchdog RayCastingShapeMask tf2_sensor_msgs_rbf ${PROJECT_NAME}_utils) ament_export_targets( export_${PROJECT_NAME} diff --git a/include/robot_body_filter/RobotBodyFilter.h b/include/robot_body_filter/RobotBodyFilter.h index a2fc266..30140db 100644 --- a/include/robot_body_filter/RobotBodyFilter.h +++ b/include/robot_body_filter/RobotBodyFilter.h @@ -8,6 +8,7 @@ #include // #include +// #include #include #include #include @@ -205,7 +206,9 @@ class RobotBodyFilter : public filters::FilterBase { std::string robotDescriptionParam; //! Subscriber for robot_description updates. - rclcpp::Subscription::SharedPtr robotDescriptionUpdatesListener; + std::shared_ptr robotDescriptionUpdatesListener; + // Callback group for dynamic updates + rclcpp::CallbackGroup::SharedPtr AsyncCallbackGroup; //! Name of the field in the dynamic reconfigure message that contains robot model. std::string robotDescriptionUpdatesFieldName; diff --git a/package.xml b/package.xml index 2212571..3d90313 100644 --- a/package.xml +++ b/package.xml @@ -18,7 +18,7 @@ filters - geometric_shapes + geometric_shapes_local geometry_msgs laser_geometry libpcl-all-dev @@ -33,6 +33,7 @@ urdf visualization_msgs pcl_conversions + urdf_parser_plugin tf2_eigen diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 184d134..d68f248 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -30,9 +30,6 @@ #include #include -// using namespace std; -// using namespace sensor_msgs; -// using namespace filters; namespace robot_body_filter { @@ -109,6 +106,7 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameter("body_model/inflation/padding", 0.0, param_desc); this->nodeHandle->declare_parameter("body_model/inflation/scale", 1.0); // NOTE: Default changed from inflationPadding/inflationScale to 0.0/1.0 + // TODO: Set defaults in GetParameter this->nodeHandle->declare_parameter("body_model/inflation/contains_test/padding", 0.0, param_desc); this->nodeHandle->declare_parameter("body_model/inflation/contains_test/scale", 1.0); this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/padding", 0.0, param_desc); @@ -123,7 +121,6 @@ void RobotBodyFilter::DeclareParameters(){ // https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1Node.html#ae5ab12777100f65bd09163814dbbf486 // this might need to be initialized with the names of each link this->nodeHandle->declare_parameters("body_model/inflation/per_link/padding", std::map()); - this->nodeHandle->declare_parameters("body_model/inflation/per_link/scale", std::map()); this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", std::vector{""}); @@ -153,12 +150,6 @@ bool RobotBodyFilter::configure() { RCLCPP_INFO(this->nodeHandle->get_logger(), "Creating TF buffer with length %f", this->tfBufferLength.seconds()); this->tfBuffer = std::make_shared(this->nodeHandle->get_clock()); this->tfListener = std::make_shared(*this->tfBuffer); - if(this->tfBuffer->canTransform("base_link", "laser", tf2::TimePointZero)){ - RCLCPP_INFO(this->nodeHandle->get_logger(), "Transform is available INSIDE"); - } - else{ - RCLCPP_INFO(this->nodeHandle->get_logger(), "Transform is not available INSIDE"); - } } else { // clear the TF buffer (useful if calling configure() after receiving old TF // data) @@ -402,18 +393,13 @@ bool RobotBodyFilter::configure() { this->nodeHandle->get_parameter("sensor/point_by_point", this->pointByPointScan); - - //TODO: Update to true ROS2 parameter callback on robot_description - // subscribe for robot_description param changes - this->robotDescriptionUpdatesListener = this->nodeHandle->template create_subscription( - "dynamic_robot_model_server/parameter_updates", 10, - std::bind(&RobotBodyFilter::robotDescriptionUpdated, this, std::placeholders::_1)); - + //TODO: Reload service // this->reloadRobotModelServiceServer = this->nodeHandle->template create_service( // "reload_model", std::bind(&RobotBodyFilter::triggerModelReload, this, std::placeholders::_1, std::placeholders::_2)); if (this->computeBoundingSphere) { - this->boundingSpherePublisher = nodeHandle->create_publisher("robot_bounding_sphere", rclcpp::QoS(100)); + this->boundingSpherePublisher = + nodeHandle->create_publisher("robot_bounding_sphere", rclcpp::QoS(100)); } if (this->computeBoundingBox) { @@ -568,27 +554,40 @@ bool RobotBodyFilter::configure() { this->tfFramesWatchdog->start(); } - { // initialize the robot body to be masked out - auto sleeper = rclcpp::Rate(rclcpp::Duration::from_seconds(1.0).nanoseconds()); - std::string robotUrdf; - while (!this->nodeHandle->get_parameter(this->robotDescriptionParam.c_str(), robotUrdf) || robotUrdf.length() == 0) { - if (this->failWithoutRobotDescription) { - throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); - } - if (!rclcpp::ok()) return false; - - RCLCPP_ERROR(this->nodeHandle->get_logger(), - "RobotBodyFilter: %s is empty or not set. Please, provide " - "the robot model. Waiting 1s. ", - robotDescriptionParam.c_str()); - rclcpp::sleep_for(std::chrono::seconds(1)); - } + AsyncCallbackGroup = nodeHandle->create_callback_group(rclcpp::CallbackGroupType::Reentrant, true); + rclcpp::QoS qos(rclcpp::KeepLast(10)); + const rmw_qos_profile_t &qos_profile = qos.get_rmw_qos_profile(); + robotDescriptionUpdatesListener = std::make_shared( + this->nodeHandle, "/robot_state_publisher", qos_profile, AsyncCallbackGroup); + auto parameters_future = robotDescriptionUpdatesListener->get_parameters( + {"robot_description"}, [this](std::shared_future> future) { + std_msgs::msg::String::SharedPtr msg = std::make_shared(); + msg->data = future.get()[0].as_string(); + RCLCPP_INFO(this->nodeHandle->get_logger(), "New URDF: %s", msg->data.c_str()); + robotDescriptionUpdated(msg); + }); - // happens when configure() is called again from update() (e.g. when - // a new bag file started - // playing) - if (!this->shapesToLinks.empty()) this->clearRobotMask(); - this->addRobotMaskFromUrdf(robotUrdf); + { // initialize the robot body to be masked out + // auto sleeper = rclcpp::Rate(rclcpp::Duration::from_seconds(1.0).nanoseconds()); + // std::string robotUrdf; + // while (!this->nodeHandle->get_parameter(this->robotDescriptionParam.c_str(), robotUrdf) || robotUrdf.length() == 0) { + // if (this->failWithoutRobotDescription) { + // throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); + // } + // if (!rclcpp::ok()) return false; + + // RCLCPP_ERROR(this->nodeHandle->get_logger(), + // "RobotBodyFilter: %s is empty or not set. Please, provide " + // "the robot model. Waiting 1s. ", + // robotDescriptionParam.c_str()); + // rclcpp::sleep_for(std::chrono::seconds(1)); + // } + + // // happens when configure() is called again from update() (e.g. when + // // a new bag file started + // // playing) + // if (!this->shapesToLinks.empty()) this->clearRobotMask(); + // this->addRobotMaskFromUrdf(robotUrdf); } RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Successfullyconfigured."); @@ -612,12 +611,7 @@ bool RobotBodyFilter::configure() { RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); } else { - // this->onlyLinks.insert("test"); - auto value2 = this->onlyLinks.find("test"); - if (value2 != this->onlyLinks.end()) { - RCLCPP_INFO(nodeHandle->get_logger(),"Did we insert element? : %s.", "yes"); - } - RCLCPP_INFO(nodeHandle->get_logger(),"Did we insert element? : %s.", "no"); + //TODO: Remove std::stringstream ss; ss << "["; size_t i = 0; @@ -640,21 +634,25 @@ bool RobotBodyFilter::configure() { this->timeConfigured = this->nodeHandle->now(); - //Bad idea? - //This should be handeled by the robotDescriptionUpdate callback - this->configured_ = true; - return true; } bool RobotBodyFilterLaserScan::configure() { - // this->nodeHandle->declare_parameter("sensor/point_by_point", true); + + RCLCPP_INFO(nodeHandle->get_logger(),"Declaring Parameters"); + //TODO: Implement + // DeclareParameters(); bool success = RobotBodyFilter::configure(); return false; } bool RobotBodyFilterPointCloud2::configure() { - RCLCPP_INFO(nodeHandle->get_logger(),"Configuring"); + + RCLCPP_INFO(nodeHandle->get_logger(),"Declaring Parameters"); + + DeclareParameters(); + + RCLCPP_INFO(nodeHandle->get_logger(),"Configuring RobotBodyFilterPointCloud2"); bool success = RobotBodyFilter::configure(); if (!success) return false; @@ -681,30 +679,25 @@ bool RobotBodyFilter::computeMask( std::vector& pointMask, const std::string& sensorFrame) { // this->modelMutex has to be already locked! - + RCLCPP_DEBUG(nodeHandle->get_logger(), "Computing Mask"); const clock_t stopwatchOverall = clock(); const auto& scanTime = projectedPointCloud.header.stamp; // compute a mask of point indices for points from projectedPointCloud // that tells if they are inside or outside robot, or shadow points - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); if (!this->pointByPointScan) { Eigen::Vector3d sensorPosition; try { - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); const auto sensorTf = this->tfBuffer->lookupTransform(this->filteringFrame, sensorFrame, scanTime, remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout)); - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); tf2::fromMsg(sensorTf.transform.translation, sensorPosition); } catch (tf2::TransformException& e) { - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); RCLCPP_ERROR(nodeHandle->get_logger(), "RobotBodyFilter: Could not compute filtering mask due to this " "TF exception: %s", e.what()); return false; } - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); // update transforms cache, which is then used in body masking this->updateTransformCache(scanTime); @@ -722,7 +715,6 @@ bool RobotBodyFilter::computeMask( CloudConstIter stamps_it(projectedPointCloud, "stamps"); pointMask.resize(num_points(projectedPointCloud)); - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); double scanDuration = 0.0; for (CloudConstIter stamps_end_it(projectedPointCloud, "stamps"); stamps_end_it != stamps_end_it.end(); @@ -741,7 +733,6 @@ bool RobotBodyFilter::computeMask( // prevent division by zero if (updateBodyPosesEvery == 0) updateBodyPosesEvery = 1; } - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); // prevent division by zero in ratio computation in case the pointcloud // isn't really taken point by point with different timestamps @@ -759,7 +750,6 @@ bool RobotBodyFilter::computeMask( Eigen::Vector3f point; Eigen::Vector3d viewPoint; RayCastingShapeMask::MaskValue mask; - RCLCPP_ERROR(nodeHandle->get_logger(), "testmsg"); this->cacheLookupBetweenScansRatio = 0.0; for (size_t i = 0; i < num_points(projectedPointCloud); @@ -802,9 +792,7 @@ bool RobotBodyFilter::computeMask( bool RobotBodyFilterLaserScan::update(const sensor_msgs::msg::LaserScan& inputScan, sensor_msgs::msg::LaserScan& filteredScan) { - RCLCPP_DEBUG(nodeHandle->get_logger(), "UPDATING MEMERS"); - const auto& headerScanTime = inputScan.header.stamp; const auto& scanTime = rclcpp::Time(inputScan.header.stamp); @@ -1023,8 +1011,6 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp return false; } - RCLCPP_ERROR(nodeHandle->get_logger(), "Passed checks"); - const auto inputCloudFrame = this->sensorFrame.empty() ? stripLeadingSlash(inputCloud.header.frame_id, true) : this->sensorFrame; @@ -1091,23 +1077,17 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } // Transform to filtering frame - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM"); - RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Transforming point cloud from input frame: %s to filtering frame: %s", inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.25"); - sensor_msgs::msg::PointCloud2 transformedCloud; if (inputCloud.header.frame_id == this->filteringFrame) { transformedCloud = inputCloud; - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.5"); } else { RCLCPP_INFO_ONCE(nodeHandle->get_logger(), "RobotBodyFilter: Transforming cloud from frame %s to %s", inputCloud.header.frame_id.c_str(), this->filteringFrame.c_str()); - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.5"); std::lock_guard guard(*this->modelMutex); std::string err; - RCLCPP_INFO(nodeHandle->get_logger(), "remaining time: %f", remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout).seconds()); if (!this->tfBuffer->canTransform(this->filteringFrame, inputCloud.header.frame_id, scanTime, remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { auto& clk = *nodeHandle->get_clock(); @@ -1115,18 +1095,15 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp "RobotBodyFilter: Cannot transform " "point cloud to filtering frame. Something's wrong with TFs: %s", err.c_str()); - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM1.75"); return false; } - RCLCPP_ERROR(nodeHandle->get_logger(), "Before Transform"); transformWithChannels(inputCloud, transformedCloud, *this->tfBuffer, this->filteringFrame, this->channelsToTransform); } - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM2"); - // Compute the mask and use it (transform message only if sensorFrame is // specified) + RCLCPP_DEBUG(nodeHandle->get_logger(), "Computing Mask"); std::vector pointMask; { std::lock_guard guard(*this->modelMutex); @@ -1136,13 +1113,14 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp } // Filter the cloud - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM3"); + RCLCPP_DEBUG(nodeHandle->get_logger(), "Filtering the cloud"); sensor_msgs::msg::PointCloud2 tmpCloud; CREATE_FILTERED_CLOUD(transformedCloud, tmpCloud, this->keepCloudsOrganized, (pointMask[i] == RayCastingShapeMask::MaskValue::OUTSIDE)) // Transform to output frame + RCLCPP_DEBUG(nodeHandle->get_logger(), "Transform to output frame"); if (tmpCloud.header.frame_id == this->outputFrame) { filteredCloud = std::move(tmpCloud); @@ -1164,7 +1142,7 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, this->outputFrame, this->channelsToTransform); } - RCLCPP_ERROR(nodeHandle->get_logger(), "TRANSFORM4"); + RCLCPP_INFO(nodeHandle->get_logger(), "Successfully updated"); return true; } @@ -1299,6 +1277,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { // add all model's collision links as masking shapes for (const auto& links : parsedUrdfModel.links_) { + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Adding link %s to the mask.", links.first.c_str()); const auto& link = links.second; // every link can have multiple collision elements @@ -1425,8 +1404,12 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { template void RobotBodyFilter::clearRobotMask() { { + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: modelMutex not yet locked."); + std::lock_guard guard(*this->modelMutex); + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: modelMutex locked."); + std::unordered_set removedMultiShapes; for (const auto& shapeToLink : this->shapesToLinks) { const auto& multiShape = shapeToLink.second.multiHandle; @@ -1520,7 +1503,7 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { if (!this->computeBoundingSphere && !this->computeDebugBoundingSphere) return; - RCLCPP_INFO(nodeHandle->get_logger(), "Computing bounding sphere"); + RCLCPP_DEBUG(nodeHandle->get_logger(), "Computing and Publishing Bounding Sphere"); // assume this->modelMutex is locked @@ -1536,21 +1519,17 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( std::vector spheres; { visualization_msgs::msg::MarkerArray boundingSphereDebugMsg; - RCLCPP_INFO(nodeHandle->get_logger(), "Compute Bounding Sphere Debug bool: %d", this->computeDebugBoundingSphere); for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingSphere()) { const auto& shapeHandle = shapeHandleAndBody.first; const auto& body = shapeHandleAndBody.second; - RCLCPP_INFO(nodeHandle->get_logger(), "Compute Bounding Sphere Debug bool: %d", this->computeDebugBoundingSphere); if (this->shapesIgnoredInBoundingSphere.find(shapeHandle) != this->shapesIgnoredInBoundingSphere.end()) continue; bodies::BoundingSphere sphere; body->computeBoundingSphere(sphere); spheres.push_back(sphere); - RCLCPP_INFO(nodeHandle->get_logger(), "Compute Bounding Sphere Debug bool: %d", this->computeDebugBoundingSphere); if (this->computeDebugBoundingSphere) { - RCLCPP_INFO(nodeHandle->get_logger(), "Computing debug sphere"); visualization_msgs::msg::Marker msg; msg.header.stamp = scanTime; msg.header.frame_id = this->filteringFrame; @@ -1802,7 +1781,7 @@ void RobotBodyFilter::computeAndPublishOrientedBoundingBox( if (this->computeOrientedBoundingBox) { bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); // TODO: fix this - // bodies::mergeBoundingBoxesApprox(boxes, box); + bodies::mergeBoundingBoxesApprox(boxes, box); // robot_body_filter::OrientedBoundingBoxStamped boundingBoxMsg; @@ -1873,18 +1852,18 @@ void RobotBodyFilter::computeAndPublishLocalBoundingBox( const auto& scanTime = projectedPointCloud.header.stamp; std::string err; - // try { - // if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, - // remainingTime(scanTime, this->reachableTransformTimeout), &err)) { - // RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), - // this->localBoundingBoxFrame.c_str(), err.c_str()); - // return; - // } - // } catch (tf2::TransformException& e) { - // RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), - // this->localBoundingBoxFrame.c_str(), e.what()); - // return; - // } + try { + if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, + remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { + RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), + this->localBoundingBoxFrame.c_str(), err.c_str()); + return; + } + } catch (tf2::TransformException& e) { + RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), + this->localBoundingBoxFrame.c_str(), e.what()); + return; + } const auto localTfMsg = this->tfBuffer->lookupTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime); const Eigen::Isometry3d localTf = tf2::transformToEigen(localTfMsg.transform); @@ -2039,14 +2018,21 @@ void RobotBodyFilter::robotDescriptionUpdated(const std_msgs::msg::String::Sh this->tfFramesWatchdog->pause(); this->configured_ = false; + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: tfWatchdog paused and configured set to false."); + this->clearRobotMask(); + + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Robot mask cleared. Reloading robot model."); + this->addRobotMaskFromUrdf(urdf); + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Robot mask succesfully added from URDF"); + this->tfFramesWatchdog->unpause(); this->timeConfigured = nodeHandle->now(); this->configured_ = true; - RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Robot model reloaded, resuming filter operation."); } template diff --git a/src/utils/tf2_sensor_msgs.cpp b/src/utils/tf2_sensor_msgs.cpp index 48197dd..c3acc80 100644 --- a/src/utils/tf2_sensor_msgs.cpp +++ b/src/utils/tf2_sensor_msgs.cpp @@ -37,10 +37,8 @@ void transformChannel(const sensor_msgs::msg::PointCloud2& cloudIn, sensor_msgs: { if (num_points(cloudIn) == 0) return; -RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "after num points"); if (type == CloudChannelType::SCALAR) return; -RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "after scalar check"); CloudConstIter x_in(cloudIn, channelPrefix + "x"); CloudConstIter y_in(cloudIn, channelPrefix + "y"); CloudConstIter z_in(cloudIn, channelPrefix + "z"); @@ -60,9 +58,7 @@ RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "after scalar check"); *x_out = point.x(); *y_out = point.y(); *z_out = point.z(); - // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "looping"); } - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "break"); break; case CloudChannelType::DIRECTION: for (; x_out != x_out.end(); ++x_in, ++y_in, ++z_in, ++x_out, ++y_out, ++z_out) @@ -71,8 +67,6 @@ RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "after scalar check"); *x_out = point.x(); *y_out = point.y(); *z_out = point.z(); - // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "looping"); - } RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "break"); break; @@ -100,7 +94,6 @@ sensor_msgs::msg::PointCloud2& transformWithChannels( const std::unordered_map& channels) { std::unordered_set channelsPresent; - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "first loop"); for (const auto& field: in.fields) { for (const auto& channelAndType : channels) { @@ -117,10 +110,8 @@ sensor_msgs::msg::PointCloud2& transformWithChannels( const auto t = tf2::transformToEigen(tf).cast(); transformChannel(in, out, t, "", CloudChannelType::POINT); - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "second loop"); for (const auto& channel : channelsPresent) transformChannel(in, out, t, channel, channels.at(channel)); - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"), "before return"); return out; } From 098283fe4b285344683a2eb5967cfc71f77b5570 Mon Sep 17 00:00:00 2001 From: seb Date: Wed, 1 May 2024 19:22:58 +0000 Subject: [PATCH 43/54] cleanup, debug msgs --- src/RobotBodyFilter.cpp | 44 ++++++++++++++++------------------------- 1 file changed, 17 insertions(+), 27 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index d68f248..c4f75c8 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -123,12 +123,12 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameters("body_model/inflation/per_link/padding", std::map()); this->nodeHandle->declare_parameters("body_model/inflation/per_link/scale", std::map()); - this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", std::vector{""}); - this->nodeHandle->declare_parameter("ignored_links/bounding_box", std::vector{""}); - this->nodeHandle->declare_parameter("ignored_links/contains_test", std::vector{""}); - this->nodeHandle->declare_parameter("ignored_links/shadow_test", std::vector{"laser"}); - this->nodeHandle->declare_parameter("ignored_links/everywhere", std::vector{""}); - this->nodeHandle->declare_parameter("only_links", std::vector{""}); + this->nodeHandle->declare_parameter("ignored_links/bounding_sphere", std::vector{}); + this->nodeHandle->declare_parameter("ignored_links/bounding_box", std::vector{}); + this->nodeHandle->declare_parameter("ignored_links/contains_test", std::vector{}); + this->nodeHandle->declare_parameter("ignored_links/shadow_test", std::vector{}); + this->nodeHandle->declare_parameter("ignored_links/everywhere", std::vector{}); + this->nodeHandle->declare_parameter("only_links", std::vector{}); this->nodeHandle->declare_parameter("body_model/dynamic_robot_description/field_name", "robot_model"); this->nodeHandle->declare_parameter("frames/output", "base_link"); @@ -137,7 +137,7 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameter("transforms/buffer_length", 60.0); //TESTING - this->nodeHandle->declare_parameter("robot_description", std::string{""}); + this->nodeHandle->declare_parameter("robot_description", std::string{}); } template @@ -599,6 +599,7 @@ bool RobotBodyFilter::configure() { if (doShadowTest) RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: \tSHADOW"); if (this->onlyLinks.empty()) { + RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: onlyLinks is empty"); if (this->linksIgnoredEverywhere.empty()) { RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to all links."); } else { @@ -611,24 +612,13 @@ bool RobotBodyFilter::configure() { RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); } else { - //TODO: Remove - std::stringstream ss; - ss << "["; - size_t i = 0; - for (const auto& v : this->onlyLinks) { - if (std::is_same::value) - ss << "\"" << to_string(v) << "\""; - else - ss << to_string(v); - if (i + 1 < this->onlyLinks.size()) ss << ", "; - ++i; - } - ss << "]"; - RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", "test", ss.str().c_str()); - auto localOnlyLinks = this->onlyLinks; - // auto onlyLinks = to_string(localOnlyLinks).c_str(); - // to_string(this->onlyLinks).c_str()); - // to_string(this->linksIgnoredEverywhere).c_str()); + //TODO: The to_string call is crashing at runtime here, dont understand why + // RCLCPP_INFO(nodeHandle->get_logger(), + // "RobotBodyFilter: Filtering applied to links %s with these links excluded: %s.", + // to_string(tempOnlyLinks).c_str(), to_string(tempLinksIgnoredEverywhere).c_str()); + for (const auto& link : this->onlyLinks) { + RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Filtering only applied to link %s.", link.c_str()); + } } } @@ -1277,7 +1267,7 @@ void RobotBodyFilter::addRobotMaskFromUrdf(const std::string& urdfModel) { // add all model's collision links as masking shapes for (const auto& links : parsedUrdfModel.links_) { - RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Adding link %s to the mask.", links.first.c_str()); + RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Adding link %s to the mask.", links.first.c_str()); const auto& link = links.second; // every link can have multiple collision elements @@ -1620,9 +1610,9 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po const auto& scanTime = projectedPointCloud.header.stamp; std::vector boxes; - { visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; + RCLCPP_INFO(nodeHandle->get_logger(), "Number of bodies: %zu", this->shapeMask->getBodiesForBoundingBox().size()); for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { const auto& shapeHandle = shapeHandleAndBody.first; const auto& body = shapeHandleAndBody.second; From dd5f6642b42b4c48cc51c8d168d89d2638468f9b Mon Sep 17 00:00:00 2001 From: seb Date: Thu, 2 May 2024 19:56:05 +0000 Subject: [PATCH 44/54] working, geometric shapes problem --- CMakeLists.txt | 2 +- package.xml | 3 +- src/RayCastingShapeMask.cpp | 8 ++--- src/RobotBodyFilter.cpp | 64 +++++++++++++++++++++---------------- 4 files changed, 43 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 27424ef..85b0cba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ else() add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -set(THIS_PACKAGE_DEPS urdf urdf_parser_plugin filters geometric_shapes_local laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen urdf visualization_msgs GLUT ) +set(THIS_PACKAGE_DEPS backward_ros urdf urdf_parser_plugin filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen urdf visualization_msgs GLUT ) set(MESSAGE_DEPS geometry_msgs std_msgs) find_package(ament_cmake REQUIRED) diff --git a/package.xml b/package.xml index 3d90313..1721747 100644 --- a/package.xml +++ b/package.xml @@ -18,7 +18,7 @@ filters - geometric_shapes_local + geometric_shapes geometry_msgs laser_geometry libpcl-all-dev @@ -34,6 +34,7 @@ visualization_msgs pcl_conversions urdf_parser_plugin + backward_ros tf2_eigen diff --git a/src/RayCastingShapeMask.cpp b/src/RayCastingShapeMask.cpp index be0a091..9b136f1 100644 --- a/src/RayCastingShapeMask.cpp +++ b/src/RayCastingShapeMask.cpp @@ -163,7 +163,7 @@ void RayCastingShapeMask::updateBodyPosesNoLock() else { if (containsBody == nullptr){ - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %s without a body", containsHandle); + // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %s without a body", containsHandle); } else { std::string name; @@ -172,11 +172,11 @@ void RayCastingShapeMask::updateBodyPosesNoLock() if (name.empty()) { - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s with handle %s", containsBody->getType(), containsHandle); + // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s with handle %s", containsBody->getType(), containsHandle); } else { - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%s)", name.c_str(), containsBody->getType()); + // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%s)", name.c_str(), containsBody->getType()); } } } @@ -354,7 +354,7 @@ MultiShapeHandle RayCastingShapeMask::addShape( const double bboxScale, const double bboxPadding, const bool updateInternalStructures, const std::string& name) { MultiShapeHandle result; - + shape->print(); result.contains = ShapeMask::addShape(shape, containsScale, containsPadding); this->data->shapeNames[result.contains] = name; diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index c4f75c8..1b4a773 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -567,28 +567,28 @@ bool RobotBodyFilter::configure() { robotDescriptionUpdated(msg); }); - { // initialize the robot body to be masked out - // auto sleeper = rclcpp::Rate(rclcpp::Duration::from_seconds(1.0).nanoseconds()); - // std::string robotUrdf; - // while (!this->nodeHandle->get_parameter(this->robotDescriptionParam.c_str(), robotUrdf) || robotUrdf.length() == 0) { - // if (this->failWithoutRobotDescription) { - // throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); - // } - // if (!rclcpp::ok()) return false; - - // RCLCPP_ERROR(this->nodeHandle->get_logger(), - // "RobotBodyFilter: %s is empty or not set. Please, provide " - // "the robot model. Waiting 1s. ", - // robotDescriptionParam.c_str()); - // rclcpp::sleep_for(std::chrono::seconds(1)); - // } - - // // happens when configure() is called again from update() (e.g. when - // // a new bag file started - // // playing) - // if (!this->shapesToLinks.empty()) this->clearRobotMask(); - // this->addRobotMaskFromUrdf(robotUrdf); - } + // { // initialize the robot body to be masked out + // auto sleeper = rclcpp::Rate(rclcpp::Duration::from_seconds(1.0).nanoseconds()); + // std::string robotUrdf; + // while (!this->nodeHandle->get_parameter(this->robotDescriptionParam.c_str(), robotUrdf) || robotUrdf.length() == 0) { + // if (this->failWithoutRobotDescription) { + // throw std::runtime_error("RobotBodyFilter: " + this->robotDescriptionParam + " is empty or not set."); + // } + // if (!rclcpp::ok()) return false; + + // RCLCPP_ERROR(this->nodeHandle->get_logger(), + // "RobotBodyFilter: %s is empty or not set. Please, provide " + // "the robot model. Waiting 1s. ", + // robotDescriptionParam.c_str()); + // rclcpp::sleep_for(std::chrono::seconds(1)); + // } + + // // happens when configure() is called again from update() (e.g. when + // // a new bag file started + // // playing) + // if (!this->shapesToLinks.empty()) this->clearRobotMask(); + // this->addRobotMaskFromUrdf(robotUrdf); + // } RCLCPP_INFO(nodeHandle->get_logger(), "RobotBodyFilter: Successfullyconfigured."); RCLCPP_INFO(nodeHandle->get_logger(), "Filtering data inframe %s", this->filteringFrame.c_str()); @@ -772,8 +772,11 @@ bool RobotBodyFilter::computeMask( this->publishDebugMarkers(scanTime); this->computeAndPublishBoundingSphere(projectedPointCloud); this->computeAndPublishBoundingBox(projectedPointCloud); - this->computeAndPublishOrientedBoundingBox(projectedPointCloud); - this->computeAndPublishLocalBoundingBox(projectedPointCloud); + RCLCPP_INFO(nodeHandle->get_logger(), "computed and published bounding box"); + // this->computeAndPublishOrientedBoundingBox(projectedPointCloud); + RCLCPP_INFO(nodeHandle->get_logger(), "computed and published oriented bounding box"); + // this->computeAndPublishLocalBoundingBox(projectedPointCloud); + RCLCPP_INFO(nodeHandle->get_logger(), "computed and published local bounding box"); RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); @@ -1526,8 +1529,6 @@ void RobotBodyFilter::computeAndPublishBoundingSphere( msg.scale.x = msg.scale.y = msg.scale.z = sphere.radius * 2; - RCLCPP_INFO(nodeHandle->get_logger(), "Sphere center: %f %f %f", sphere.center[0], sphere.center[1], sphere.center[2]); - msg.pose.position.x = sphere.center[0]; msg.pose.position.y = sphere.center[1]; msg.pose.position.z = sphere.center[2]; @@ -1614,13 +1615,17 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; RCLCPP_INFO(nodeHandle->get_logger(), "Number of bodies: %zu", this->shapeMask->getBodiesForBoundingBox().size()); for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { + RCLCPP_INFO(nodeHandle->get_logger(), "declaring variables"); const auto& shapeHandle = shapeHandleAndBody.first; const auto& body = shapeHandleAndBody.second; - + RCLCPP_INFO(nodeHandle->get_logger(), "variables declared"); + // RCLCPP_INFO(nodeHandle->get_logger(), "Looking for shape handle %s", to_string(shapeHandle).c_str()); if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; bodies::AxisAlignedBoundingBox box; + // RCLCPP_INFO(nodeHandle->get_logger(), "Computing bounding box for shape %s", to_string(shapeHandle).c_str()); body->computeBoundingBox(box); + RCLCPP_INFO(nodeHandle->get_logger(), "Computed"); boxes.push_back(box); @@ -1646,6 +1651,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po } if (this->computeDebugBoundingBox) { + RCLCPP_INFO(nodeHandle->get_logger(), "Publishing bounding box debug markers"); this->boundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); } } @@ -1664,6 +1670,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po tf2::toMsg(box.min(), boundingBoxMsg.polygon.points[0]); tf2::toMsg(box.max(), boundingBoxMsg.polygon.points[1]); + RCLCPP_INFO(nodeHandle->get_logger(), "Publishing bounding box message"); this->boundingBoxPublisher->publish(boundingBoxMsg); if (this->publishBoundingBoxMarker) { @@ -1683,6 +1690,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po msg.ns = "bounding_box"; msg.frame_locked = static_cast(true); + RCLCPP_INFO(nodeHandle->get_logger(), "Publishing bounding box Marker"); this->boundingBoxMarkerPublisher->publish(msg); } @@ -1705,7 +1713,7 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - + RCLCPP_INFO(nodeHandle->get_logger(), "Publish scan cloud"); this->scanPointCloudNoBoundingBoxPublisher->publish(*boxFilteredCloud); } } From 238f2aa5a4166c4d7adb5397efffd4b9e6c4fa85 Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 6 May 2024 16:55:10 -0400 Subject: [PATCH 45/54] disabling obb dependent code --- src/RobotBodyFilter.cpp | 494 ++++++++++++++++++++-------------------- 1 file changed, 247 insertions(+), 247 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 1b4a773..e967a37 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1719,254 +1719,254 @@ void RobotBodyFilter::computeAndPublishBoundingBox(const sensor_msgs::msg::Po } } -template -void RobotBodyFilter::computeAndPublishOrientedBoundingBox( - const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { - if (!this->computeOrientedBoundingBox && !this->computeDebugOrientedBoundingBox) return; - - // assume this->modelMutex is locked - - // when computing bounding boxes for publication, we want to publish them to - // the time of the scan, so we need to set cacheLookupBetweenScansRatio again - // to zero - if (this->cacheLookupBetweenScansRatio != 0.0) { - this->cacheLookupBetweenScansRatio = 0.0; - this->shapeMask->updateBodyPoses(); - } - - const auto& scanTime = projectedPointCloud.header.stamp; - std::vector boxes; - - { - visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; - for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { - const auto& shapeHandle = shapeHandleAndBody.first; - const auto& body = shapeHandleAndBody.second; - - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) - continue; - - bodies::OrientedBoundingBox box; - body->computeBoundingBox(box); - - boxes.push_back(box); - - if (this->computeDebugOrientedBoundingBox) { - visualization_msgs::msg::Marker msg; - msg.header.stamp = scanTime; - msg.header.frame_id = this->filteringFrame; - - tf2::toMsg(box.getExtents(), msg.scale); - msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); - msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); - - msg.color.g = 1.0; - msg.color.a = 0.5; - msg.type = visualization_msgs::msg::Marker::CUBE; - msg.action = visualization_msgs::msg::Marker::ADD; - msg.ns = "obbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; - msg.frame_locked = static_cast(true); - - boundingBoxDebugMsg.markers.push_back(msg); - } - } - - if (this->computeDebugOrientedBoundingBox) { - this->orientedBoundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); - } - } - - if (this->computeOrientedBoundingBox) { - bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); - // TODO: fix this - bodies::mergeBoundingBoxesApprox(boxes, box); - - // robot_body_filter::OrientedBoundingBoxStamped boundingBoxMsg; - - // boundingBoxMsg.header.stamp = scanTime; - // boundingBoxMsg.header.frame_id = this->filteringFrame; - - // tf2::toMsg(box.getExtents(), boundingBoxMsg.obb.extents); - // tf2::toMsg(box.getPose().translation(), - // boundingBoxMsg.obb.pose.translation); - // boundingBoxMsg.obb.pose.rotation = - // tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); - - // this->orientedBoundingBoxPublisher.publish(boundingBoxMsg); - - if (this->publishOrientedBoundingBoxMarker) { - visualization_msgs::msg::Marker msg; - msg.header.stamp = scanTime; - msg.header.frame_id = this->filteringFrame; - - tf2::toMsg(box.getExtents(), msg.scale); - msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); - msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); - - msg.color.r = 1.0; - msg.color.a = 0.5; - msg.type = visualization_msgs::msg::Marker::CUBE; - msg.action = visualization_msgs::msg::Marker::ADD; - msg.ns = "oriented_bounding_box"; - msg.frame_locked = static_cast(true); - - this->orientedBoundingBoxMarkerPublisher->publish(msg); - } - - // compute and publish the scan_point_cloud with robot bounding box removed - if (this->publishNoOrientedBoundingBoxPointcloud) { - pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); - pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); - - robot_body_filter::CropBoxPointCloud2 cropBox; - cropBox.setNegative(true); - cropBox.setInputCloud(bboxCropInput); - cropBox.setKeepOrganized(this->keepCloudsOrganized); - - const auto e = box.getExtents(); - cropBox.setMin(Eigen::Vector4f(-e.x() / 2, -e.y() / 2, -e.z() / 2, 0.0)); - cropBox.setMax(Eigen::Vector4f(e.x() / 2, e.y() / 2, e.z() / 2, 0.0)); - cropBox.setTranslation(box.getPose().translation().cast()); - cropBox.setRotation(box.getPose().linear().eulerAngles(0, 1, 2).cast()); - - pcl::PCLPointCloud2 pclOutput; - cropBox.filter(pclOutput); - - sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); - pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - - this->scanPointCloudNoOrientedBoundingBoxPublisher->publish(*boxFilteredCloud); - } - } -} - -template -void RobotBodyFilter::computeAndPublishLocalBoundingBox( - const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { - if (!this->computeLocalBoundingBox && !this->computeDebugLocalBoundingBox) return; - - // assume this->modelMutex is locked +// template +// void RobotBodyFilter::computeAndPublishOrientedBoundingBox( +// const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { +// if (!this->computeOrientedBoundingBox && !this->computeDebugOrientedBoundingBox) return; + +// // assume this->modelMutex is locked + +// // when computing bounding boxes for publication, we want to publish them to +// // the time of the scan, so we need to set cacheLookupBetweenScansRatio again +// // to zero +// if (this->cacheLookupBetweenScansRatio != 0.0) { +// this->cacheLookupBetweenScansRatio = 0.0; +// this->shapeMask->updateBodyPoses(); +// } + +// const auto& scanTime = projectedPointCloud.header.stamp; +// std::vector boxes; + +// { +// visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; +// for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { +// const auto& shapeHandle = shapeHandleAndBody.first; +// const auto& body = shapeHandleAndBody.second; + +// if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) +// continue; + +// bodies::OrientedBoundingBox box; +// body->computeBoundingBox(box); + +// boxes.push_back(box); + +// if (this->computeDebugOrientedBoundingBox) { +// visualization_msgs::msg::Marker msg; +// msg.header.stamp = scanTime; +// msg.header.frame_id = this->filteringFrame; + +// tf2::toMsg(box.getExtents(), msg.scale); +// msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); +// msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + +// msg.color.g = 1.0; +// msg.color.a = 0.5; +// msg.type = visualization_msgs::msg::Marker::CUBE; +// msg.action = visualization_msgs::msg::Marker::ADD; +// msg.ns = "obbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; +// msg.frame_locked = static_cast(true); + +// boundingBoxDebugMsg.markers.push_back(msg); +// } +// } + +// if (this->computeDebugOrientedBoundingBox) { +// this->orientedBoundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); +// } +// } + +// if (this->computeOrientedBoundingBox) { +// bodies::OrientedBoundingBox box(Eigen::Isometry3d::Identity(), Eigen::Vector3d::Zero()); +// // TODO: fix this +// bodies::mergeBoundingBoxesApprox(boxes, box); + +// // robot_body_filter::OrientedBoundingBoxStamped boundingBoxMsg; + +// // boundingBoxMsg.header.stamp = scanTime; +// // boundingBoxMsg.header.frame_id = this->filteringFrame; + +// // tf2::toMsg(box.getExtents(), boundingBoxMsg.obb.extents); +// // tf2::toMsg(box.getPose().translation(), +// // boundingBoxMsg.obb.pose.translation); +// // boundingBoxMsg.obb.pose.rotation = +// // tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + +// // this->orientedBoundingBoxPublisher.publish(boundingBoxMsg); + +// if (this->publishOrientedBoundingBoxMarker) { +// visualization_msgs::msg::Marker msg; +// msg.header.stamp = scanTime; +// msg.header.frame_id = this->filteringFrame; + +// tf2::toMsg(box.getExtents(), msg.scale); +// msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.getPose().translation()); +// msg.pose.orientation = tf2::toMsg(Eigen::Quaterniond(box.getPose().linear())); + +// msg.color.r = 1.0; +// msg.color.a = 0.5; +// msg.type = visualization_msgs::msg::Marker::CUBE; +// msg.action = visualization_msgs::msg::Marker::ADD; +// msg.ns = "oriented_bounding_box"; +// msg.frame_locked = static_cast(true); + +// this->orientedBoundingBoxMarkerPublisher->publish(msg); +// } + +// // compute and publish the scan_point_cloud with robot bounding box removed +// if (this->publishNoOrientedBoundingBoxPointcloud) { +// pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); +// pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); + +// robot_body_filter::CropBoxPointCloud2 cropBox; +// cropBox.setNegative(true); +// cropBox.setInputCloud(bboxCropInput); +// cropBox.setKeepOrganized(this->keepCloudsOrganized); + +// const auto e = box.getExtents(); +// cropBox.setMin(Eigen::Vector4f(-e.x() / 2, -e.y() / 2, -e.z() / 2, 0.0)); +// cropBox.setMax(Eigen::Vector4f(e.x() / 2, e.y() / 2, e.z() / 2, 0.0)); +// cropBox.setTranslation(box.getPose().translation().cast()); +// cropBox.setRotation(box.getPose().linear().eulerAngles(0, 1, 2).cast()); + +// pcl::PCLPointCloud2 pclOutput; +// cropBox.filter(pclOutput); + +// sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); +// pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); +// boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp + +// this->scanPointCloudNoOrientedBoundingBoxPublisher->publish(*boxFilteredCloud); +// } +// } +// } + +// template +// void RobotBodyFilter::computeAndPublishLocalBoundingBox( +// const sensor_msgs::msg::PointCloud2& projectedPointCloud) const { +// if (!this->computeLocalBoundingBox && !this->computeDebugLocalBoundingBox) return; + +// // assume this->modelMutex is locked + +// const auto& scanTime = projectedPointCloud.header.stamp; +// std::string err; +// try { +// if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, +// remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { +// RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), +// this->localBoundingBoxFrame.c_str(), err.c_str()); +// return; +// } +// } catch (tf2::TransformException& e) { +// RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), +// this->localBoundingBoxFrame.c_str(), e.what()); +// return; +// } + +// const auto localTfMsg = this->tfBuffer->lookupTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime); +// const Eigen::Isometry3d localTf = tf2::transformToEigen(localTfMsg.transform); + +// std::vector boxes; + +// { +// visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; +// for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { +// const auto& shapeHandle = shapeHandleAndBody.first; +// const auto& body = shapeHandleAndBody.second; + +// if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; + +// bodies::AxisAlignedBoundingBox box; +// bodies::computeBoundingBoxAt(body, box, localTf * body->getPose()); + +// boxes.push_back(box); + +// if (this->computeDebugLocalBoundingBox) { +// visualization_msgs::msg::Marker msg; +// msg.header.stamp = scanTime; +// msg.header.frame_id = this->localBoundingBoxFrame; + +// tf2::toMsg(box.sizes(), msg.scale); +// msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.center()); +// msg.pose.orientation.w = 1; + +// msg.color.g = 1.0; +// msg.color.a = 0.5; +// msg.type = visualization_msgs::msg::Marker::CUBE; +// msg.action = visualization_msgs::msg::Marker::ADD; +// msg.ns = "lbbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; +// msg.frame_locked = static_cast(true); + +// boundingBoxDebugMsg.markers.push_back(msg); +// } +// } + +// if (this->computeDebugLocalBoundingBox) { +// this->localBoundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); +// } +// } + +// if (this->computeLocalBoundingBox) { +// bodies::AxisAlignedBoundingBox box; +// bodies::mergeBoundingBoxes(boxes, box); + +// geometry_msgs::msg::PolygonStamped boundingBoxMsg; + +// boundingBoxMsg.header.stamp = scanTime; +// boundingBoxMsg.header.frame_id = this->localBoundingBoxFrame; + +// boundingBoxMsg.polygon.points.resize(2); +// tf2::toMsg(box.min(), boundingBoxMsg.polygon.points[0]); +// tf2::toMsg(box.max(), boundingBoxMsg.polygon.points[1]); + +// this->localBoundingBoxPublisher->publish(boundingBoxMsg); + +// if (this->publishLocalBoundingBoxMarker) { +// visualization_msgs::msg::Marker msg; +// msg.header.stamp = scanTime; +// msg.header.frame_id = this->localBoundingBoxFrame; + +// tf2::toMsg(box.sizes(), msg.scale); +// msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.center()); +// msg.pose.orientation.w = 1; + +// msg.color.r = 1.0; +// msg.color.a = 0.5; +// msg.type = visualization_msgs::msg::Marker::CUBE; +// msg.action = visualization_msgs::msg::Marker::ADD; +// msg.ns = "local_bounding_box"; +// msg.frame_locked = static_cast(true); + +// this->localBoundingBoxMarkerPublisher->publish(msg); +// } + +// // compute and publish the scan_point_cloud with robot bounding box removed +// if (this->publishNoLocalBoundingBoxPointcloud) { +// pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); +// pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); + +// robot_body_filter::CropBoxPointCloud2 cropBox; +// cropBox.setNegative(true); +// cropBox.setInputCloud(bboxCropInput); +// cropBox.setKeepOrganized(this->keepCloudsOrganized); + +// cropBox.setMin(Eigen::Vector4f(box.min()[0], box.min()[1], box.min()[2], 0.0)); +// cropBox.setMax(Eigen::Vector4f(box.max()[0], box.max()[1], box.max()[2], 0.0)); +// const Eigen::Isometry3d localTfInv = localTf.inverse(); +// cropBox.setTranslation(localTfInv.translation().cast()); +// cropBox.setRotation(localTfInv.linear().eulerAngles(0, 1, 2).cast()); + +// pcl::PCLPointCloud2 pclOutput; +// cropBox.filter(pclOutput); + +// sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); +// pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); +// boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - const auto& scanTime = projectedPointCloud.header.stamp; - std::string err; - try { - if (!this->tfBuffer->canTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime, - remainingTime(*this->nodeHandle->get_clock(), scanTime, this->reachableTransformTimeout), &err)) { - RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), err.c_str()); - return; - } - } catch (tf2::TransformException& e) { - RCLCPP_ERROR(nodeHandle->get_logger(), "Cannot get transform %s->%s. Error is %s.", this->filteringFrame.c_str(), - this->localBoundingBoxFrame.c_str(), e.what()); - return; - } - - const auto localTfMsg = this->tfBuffer->lookupTransform(this->localBoundingBoxFrame, this->filteringFrame, scanTime); - const Eigen::Isometry3d localTf = tf2::transformToEigen(localTfMsg.transform); - - std::vector boxes; - - { - visualization_msgs::msg::MarkerArray boundingBoxDebugMsg; - for (const auto& shapeHandleAndBody : this->shapeMask->getBodiesForBoundingBox()) { - const auto& shapeHandle = shapeHandleAndBody.first; - const auto& body = shapeHandleAndBody.second; - - if (this->shapesIgnoredInBoundingBox.find(shapeHandle) != this->shapesIgnoredInBoundingBox.end()) continue; - - bodies::AxisAlignedBoundingBox box; - bodies::computeBoundingBoxAt(body, box, localTf * body->getPose()); - - boxes.push_back(box); - - if (this->computeDebugLocalBoundingBox) { - visualization_msgs::msg::Marker msg; - msg.header.stamp = scanTime; - msg.header.frame_id = this->localBoundingBoxFrame; - - tf2::toMsg(box.sizes(), msg.scale); - msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.center()); - msg.pose.orientation.w = 1; - - msg.color.g = 1.0; - msg.color.a = 0.5; - msg.type = visualization_msgs::msg::Marker::CUBE; - msg.action = visualization_msgs::msg::Marker::ADD; - msg.ns = "lbbox/" + this->shapesToLinks.at(shapeHandle).cacheKey; - msg.frame_locked = static_cast(true); - - boundingBoxDebugMsg.markers.push_back(msg); - } - } - - if (this->computeDebugLocalBoundingBox) { - this->localBoundingBoxDebugMarkerPublisher->publish(boundingBoxDebugMsg); - } - } - - if (this->computeLocalBoundingBox) { - bodies::AxisAlignedBoundingBox box; - bodies::mergeBoundingBoxes(boxes, box); - - geometry_msgs::msg::PolygonStamped boundingBoxMsg; - - boundingBoxMsg.header.stamp = scanTime; - boundingBoxMsg.header.frame_id = this->localBoundingBoxFrame; - - boundingBoxMsg.polygon.points.resize(2); - tf2::toMsg(box.min(), boundingBoxMsg.polygon.points[0]); - tf2::toMsg(box.max(), boundingBoxMsg.polygon.points[1]); - - this->localBoundingBoxPublisher->publish(boundingBoxMsg); - - if (this->publishLocalBoundingBoxMarker) { - visualization_msgs::msg::Marker msg; - msg.header.stamp = scanTime; - msg.header.frame_id = this->localBoundingBoxFrame; - - tf2::toMsg(box.sizes(), msg.scale); - msg.pose.position = tf2::toMsg((Eigen::Vector3d)box.center()); - msg.pose.orientation.w = 1; - - msg.color.r = 1.0; - msg.color.a = 0.5; - msg.type = visualization_msgs::msg::Marker::CUBE; - msg.action = visualization_msgs::msg::Marker::ADD; - msg.ns = "local_bounding_box"; - msg.frame_locked = static_cast(true); - - this->localBoundingBoxMarkerPublisher->publish(msg); - } - - // compute and publish the scan_point_cloud with robot bounding box removed - if (this->publishNoLocalBoundingBoxPointcloud) { - pcl::PCLPointCloud2::Ptr bboxCropInput(new pcl::PCLPointCloud2()); - pcl_conversions::toPCL(projectedPointCloud, *(bboxCropInput)); - - robot_body_filter::CropBoxPointCloud2 cropBox; - cropBox.setNegative(true); - cropBox.setInputCloud(bboxCropInput); - cropBox.setKeepOrganized(this->keepCloudsOrganized); - - cropBox.setMin(Eigen::Vector4f(box.min()[0], box.min()[1], box.min()[2], 0.0)); - cropBox.setMax(Eigen::Vector4f(box.max()[0], box.max()[1], box.max()[2], 0.0)); - const Eigen::Isometry3d localTfInv = localTf.inverse(); - cropBox.setTranslation(localTfInv.translation().cast()); - cropBox.setRotation(localTfInv.linear().eulerAngles(0, 1, 2).cast()); - - pcl::PCLPointCloud2 pclOutput; - cropBox.filter(pclOutput); - - sensor_msgs::msg::PointCloud2::SharedPtr boxFilteredCloud(new sensor_msgs::msg::PointCloud2()); - pcl_conversions::moveFromPCL(pclOutput, *boxFilteredCloud); - boxFilteredCloud->header.stamp = scanTime; // PCL strips precision of timestamp - - this->scanPointCloudNoLocalBoundingBoxPublisher->publish(*boxFilteredCloud); - } - } -} +// this->scanPointCloudNoLocalBoundingBoxPublisher->publish(*boxFilteredCloud); +// } +// } +// } template void RobotBodyFilter::createBodyVisualizationMsg( From 8e736e462300b4a617c6285e6072701ba49f7197 Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 6 May 2024 16:55:35 -0400 Subject: [PATCH 46/54] disabled obb related --- include/robot_body_filter/utils/bodies.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/robot_body_filter/utils/bodies.h b/include/robot_body_filter/utils/bodies.h index 5d90ac9..af4de37 100644 --- a/include/robot_body_filter/utils/bodies.h +++ b/include/robot_body_filter/utils/bodies.h @@ -17,7 +17,7 @@ namespace bodies { typedef bodies::AABB AxisAlignedBoundingBox; -typedef bodies::OBB OrientedBoundingBox; +// typedef bodies::OBB OrientedBoundingBox; /** \brief Compute AABB for the body at different pose. Can't use setPose() because we want `body` to be const. */ void computeBoundingBoxAt(const bodies::Body* body, AxisAlignedBoundingBox& bbox, const Eigen::Isometry3d& pose); From db88ebc01cd6c1392337c739aedaae0bb22bb2b3 Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 6 May 2024 16:55:45 -0400 Subject: [PATCH 47/54] debug msgs cleanup --- src/RobotBodyFilter.cpp | 92 +++++++++++------------------------------ 1 file changed, 25 insertions(+), 67 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index e967a37..2fd80d5 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -45,8 +45,10 @@ RobotBodyFilter::RobotBodyFilter(std::shared_ptr inputNode) } void RobotBodyFilterPointCloud2::DeclareParameters(){ - this->nodeHandle->declare_parameter("sensor/point_by_point", false); - RobotBodyFilter::DeclareParameters(); + if (this->nodeHandle->has_parameter("sensor/point_by_point") == false){ + this->nodeHandle->declare_parameter("sensor/point_by_point", false); + RobotBodyFilter::DeclareParameters(); + } }; template @@ -104,17 +106,17 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameter("debug/marker/bounding_box", false); param_desc.description = "m"; this->nodeHandle->declare_parameter("body_model/inflation/padding", 0.0, param_desc); - this->nodeHandle->declare_parameter("body_model/inflation/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/scale", 1.1); // NOTE: Default changed from inflationPadding/inflationScale to 0.0/1.0 // TODO: Set defaults in GetParameter - this->nodeHandle->declare_parameter("body_model/inflation/contains_test/padding", 0.0, param_desc); - this->nodeHandle->declare_parameter("body_model/inflation/contains_test/scale", 1.0); - this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/padding", 0.0, param_desc); - this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/scale", 1.0); - this->nodeHandle->declare_parameter("body_model/inflation/bounding_sphere/padding", 0.0, param_desc); - this->nodeHandle->declare_parameter("body_model/inflation/bounding_sphere/scale", 1.0); - this->nodeHandle->declare_parameter("body_model/inflation/bounding_box/padding", 0.0, param_desc); - this->nodeHandle->declare_parameter("body_model/inflation/bounding_box/scale", 1.0); + this->nodeHandle->declare_parameter("body_model/inflation/contains_test/padding", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/contains_test/scale", rclcpp::ParameterType::PARAMETER_DOUBLE); + this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/padding", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/shadow_test/scale", rclcpp::ParameterType::PARAMETER_DOUBLE); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_sphere/padding", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_sphere/scale", rclcpp::ParameterType::PARAMETER_DOUBLE); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_box/padding", rclcpp::ParameterType::PARAMETER_DOUBLE, param_desc); + this->nodeHandle->declare_parameter("body_model/inflation/bounding_box/scale", rclcpp::ParameterType::PARAMETER_DOUBLE); // TODO: This initialization may be incorrect, not sure if I understand how // this works @@ -169,114 +171,72 @@ bool RobotBodyFilter::configure() { this->nodeHandle->get_parameter("maxDistance", this->maxDistance); this->nodeHandle->get_parameter("body_model/robot_description_param", this->robotDescriptionParam); this->nodeHandle->get_parameter("filter/keep_clouds_organized", this->keepCloudsOrganized); - double tempModelPoseUpdateInterval; this->nodeHandle->get_parameter("filter/model_pose_update_interval", tempModelPoseUpdateInterval); this->modelPoseUpdateInterval = rclcpp::Duration::from_seconds(tempModelPoseUpdateInterval); - bool tempDoClipping; this->nodeHandle->get_parameter("filter/do_clipping", tempDoClipping); const bool doClipping = tempDoClipping; - bool tempDoContainsTest; this->nodeHandle->get_parameter("filter/do_contains_test", tempDoContainsTest); const bool doContainsTest = tempDoContainsTest; - bool tempDoShadowTest; this->nodeHandle->get_parameter("filter/do_shadow_test", tempDoShadowTest); const bool doShadowTest = tempDoShadowTest; - double tempMaxShadowDistance; this->nodeHandle->get_parameter_or("filter/max_shadow_distance", tempMaxShadowDistance, this->maxDistance); const double maxShadowDistance = tempMaxShadowDistance; - double tempReachableTransformTimeout; this->nodeHandle->get_parameter("transforms/timeout/reachable", tempReachableTransformTimeout); this->reachableTransformTimeout = rclcpp::Duration::from_seconds(tempReachableTransformTimeout); - double tempUnreachableTransformTimeout; this->nodeHandle->get_parameter("transforms/timeout/unreachable", tempUnreachableTransformTimeout); this->unreachableTransformTimeout = rclcpp::Duration::from_seconds(tempUnreachableTransformTimeout); - this->nodeHandle->get_parameter("transforms/require_all_reachable", this->requireAllFramesReachable); - this->nodeHandle->get_parameter("bounding_sphere/publish_cut_out_pointcloud", this->publishNoBoundingSpherePointcloud); - this->nodeHandle->get_parameter("bounding_box/publish_cut_out_pointcloud", this->publishNoBoundingBoxPointcloud); - this->nodeHandle->get_parameter("oriented_bounding_box/publish_cut_out_pointcloud", this->publishNoOrientedBoundingBoxPointcloud); - this->nodeHandle->get_parameter("local_bounding_box/publish_cut_out_pointcloud", this->publishNoLocalBoundingBoxPointcloud); - this->nodeHandle->get_parameter("bounding_sphere/compute", this->computeBoundingSphere); this->computeBoundingSphere = this->computeBoundingSphere || this->publishNoBoundingSpherePointcloud; - this->nodeHandle->get_parameter("bounding_box/compute", this->computeBoundingBox); this->computeBoundingBox = this->computeBoundingBox || this->publishNoBoundingBoxPointcloud; - this->nodeHandle->get_parameter("oriented_bounding_box/compute", this->computeOrientedBoundingBox); this->computeOrientedBoundingBox = this->computeOrientedBoundingBox || this->publishNoOrientedBoundingBoxPointcloud; - this->nodeHandle->get_parameter("local_bounding_box/compute", this->computeLocalBoundingBox); this->computeLocalBoundingBox = this->computeLocalBoundingBox || this->publishNoLocalBoundingBoxPointcloud; - this->nodeHandle->get_parameter("bounding_sphere/debug", this->computeDebugBoundingSphere); - this->nodeHandle->get_parameter("bounding_box/debug", this->computeDebugBoundingBox); - this->nodeHandle->get_parameter("oriented_bounding_box/debug", this->computeDebugOrientedBoundingBox); - this->nodeHandle->get_parameter("local_bounding_box/debug", this->computeDebugLocalBoundingBox); - this->nodeHandle->get_parameter("bounding_sphere/marker", this->publishBoundingSphereMarker); - this->nodeHandle->get_parameter("bounding_box/marker", this->publishBoundingBoxMarker); - this->nodeHandle->get_parameter("oriented_bounding_box/marker", this->publishOrientedBoundingBoxMarker); - this->nodeHandle->get_parameter("local_bounding_box/marker", this->publishLocalBoundingBoxMarker); - this->nodeHandle->get_parameter_or("local_bounding_box/frame_id", this->localBoundingBoxFrame, this->fixedFrame); - this->nodeHandle->get_parameter("debug/pcl/inside", this->publishDebugPclInside); - this->nodeHandle->get_parameter("debug/pcl/clip", this->publishDebugPclClip); - this->nodeHandle->get_parameter("debug/pcl/shadow", this->publishDebugPclShadow); - this->nodeHandle->get_parameter("debug/marker/contains", this->publishDebugContainsMarker); - this->nodeHandle->get_parameter("debug/marker/shadow", this->publishDebugShadowMarker); - this->nodeHandle->get_parameter("debug/marker/bounding_sphere", this->publishDebugBsphereMarker); - this->nodeHandle->get_parameter("debug/marker/bounding_box", this->publishDebugBboxMarker); - double tempInflationPadding; this->nodeHandle->get_parameter("body_model/inflation/padding", tempInflationPadding); const double inflationPadding = tempInflationPadding; - double tempInflationScale; this->nodeHandle->get_parameter("body_model/inflation/scale", tempInflationScale); const double inflationScale = tempInflationScale; - - this->nodeHandle->get_parameter("body_model/inflation/contains_test/padding", this->defaultContainsInflation.padding); - - this->nodeHandle->get_parameter("body_model/inflation/contains_test/scale", this->defaultContainsInflation.scale); - - this->nodeHandle->get_parameter("body_model/inflation/shadow_test/padding", this->defaultShadowInflation.padding); - - this->nodeHandle->get_parameter("body_model/inflation/shadow_test/scale", this->defaultShadowInflation.scale); - - this->nodeHandle->get_parameter("body_model/inflation/bounding_sphere/padding", this->defaultBsphereInflation.padding); - - this->nodeHandle->get_parameter("body_model/inflation/bounding_sphere/scale", this->defaultBsphereInflation.scale); - - this->nodeHandle->get_parameter("body_model/inflation/bounding_box/padding", this->defaultBboxInflation.padding); - - this->nodeHandle->get_parameter("body_model/inflation/bounding_box/scale", this->defaultBboxInflation.scale); + this->nodeHandle->get_parameter_or("body_model/inflation/contains_test/padding", this->defaultContainsInflation.padding, inflationPadding); + this->nodeHandle->get_parameter_or("body_model/inflation/contains_test/scale", this->defaultContainsInflation.scale, inflationScale); + this->nodeHandle->get_parameter_or("body_model/inflation/shadow_test/padding", this->defaultShadowInflation.padding, inflationPadding); + this->nodeHandle->get_parameter_or("body_model/inflation/shadow_test/scale", this->defaultShadowInflation.scale, inflationScale); + this->nodeHandle->get_parameter_or("body_model/inflation/bounding_sphere/padding", this->defaultBsphereInflation.padding, inflationPadding); + this->nodeHandle->get_parameter_or("body_model/inflation/bounding_sphere/scale", this->defaultBsphereInflation.scale, inflationScale); + this->nodeHandle->get_parameter_or("body_model/inflation/bounding_box/padding", this->defaultBboxInflation.padding, inflationPadding); + this->nodeHandle->get_parameter_or("body_model/inflation/bounding_box/scale", this->defaultBboxInflation.scale, inflationScale); // read per-link padding std::map perLinkInflationPadding; @@ -609,7 +569,7 @@ bool RobotBodyFilter::configure() { } else { if (this->linksIgnoredEverywhere.empty()) { - RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); + // RCLCPP_INFO(nodeHandle->get_logger(),"RobotBodyFilter: Filtering applied to links %s.", to_string(this->onlyLinks).c_str()); } else { //TODO: The to_string call is crashing at runtime here, dont understand why @@ -772,11 +732,11 @@ bool RobotBodyFilter::computeMask( this->publishDebugMarkers(scanTime); this->computeAndPublishBoundingSphere(projectedPointCloud); this->computeAndPublishBoundingBox(projectedPointCloud); - RCLCPP_INFO(nodeHandle->get_logger(), "computed and published bounding box"); + RCLCPP_DEBUG(nodeHandle->get_logger(), "computed and published bounding box"); // this->computeAndPublishOrientedBoundingBox(projectedPointCloud); - RCLCPP_INFO(nodeHandle->get_logger(), "computed and published oriented bounding box"); + RCLCPP_DEBUG(nodeHandle->get_logger(), "computed and published oriented bounding box"); // this->computeAndPublishLocalBoundingBox(projectedPointCloud); - RCLCPP_INFO(nodeHandle->get_logger(), "computed and published local bounding box"); + RCLCPP_DEBUG(nodeHandle->get_logger(), "computed and published local bounding box"); RCLCPP_DEBUG(nodeHandle->get_logger(), "RobotBodyFilter: Filtering run time is %.5f secs.", double(clock() - stopwatchOverall) / CLOCKS_PER_SEC); @@ -1135,8 +1095,6 @@ bool RobotBodyFilterPointCloud2::update(const sensor_msgs::msg::PointCloud2& inp transformWithChannels(tmpCloud, filteredCloud, *this->tfBuffer, this->outputFrame, this->channelsToTransform); } - RCLCPP_INFO(nodeHandle->get_logger(), "Successfully updated"); - return true; } From 57ec132f7d42ef0ccdd3190398fe3077d424445e Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 6 May 2024 17:40:43 -0400 Subject: [PATCH 48/54] debug msgs --- src/RayCastingShapeMask.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/RayCastingShapeMask.cpp b/src/RayCastingShapeMask.cpp index 9b136f1..f901dd7 100644 --- a/src/RayCastingShapeMask.cpp +++ b/src/RayCastingShapeMask.cpp @@ -163,7 +163,7 @@ void RayCastingShapeMask::updateBodyPosesNoLock() else { if (containsBody == nullptr){ - // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %s without a body", containsHandle); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %s without a body", containsHandle); } else { std::string name; @@ -172,11 +172,11 @@ void RayCastingShapeMask::updateBodyPosesNoLock() if (name.empty()) { - // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s with handle %s", containsBody->getType(), containsHandle); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s with handle %s", containsBody->getType(), containsHandle); } else { - // RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%s)", name.c_str(), containsBody->getType()); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%s)", name.c_str(), containsBody->getType()); } } } From 7eff08ed68a13ac69df81806811346289a177339 Mon Sep 17 00:00:00 2001 From: seb Date: Mon, 6 May 2024 17:44:39 -0400 Subject: [PATCH 49/54] cleanup --- src/RayCastingShapeMask.cpp | 2 +- src/RobotBodyFilter.cpp | 4 ---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/src/RayCastingShapeMask.cpp b/src/RayCastingShapeMask.cpp index f901dd7..be0a091 100644 --- a/src/RayCastingShapeMask.cpp +++ b/src/RayCastingShapeMask.cpp @@ -354,7 +354,7 @@ MultiShapeHandle RayCastingShapeMask::addShape( const double bboxScale, const double bboxPadding, const bool updateInternalStructures, const std::string& name) { MultiShapeHandle result; - shape->print(); + result.contains = ShapeMask::addShape(shape, containsScale, containsPadding); this->data->shapeNames[result.contains] = name; diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 2fd80d5..4b80424 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -138,7 +138,6 @@ void RobotBodyFilter::DeclareParameters(){ this->nodeHandle->declare_parameter("cloud/direction_channels", std::vector{"normal_"}); this->nodeHandle->declare_parameter("transforms/buffer_length", 60.0); - //TESTING this->nodeHandle->declare_parameter("robot_description", std::string{}); } @@ -159,13 +158,10 @@ bool RobotBodyFilter::configure() { } this->nodeHandle->get_parameter("fixedFrame", this->fixedFrame); - RCLCPP_DEBUG(this->nodeHandle->get_logger(), "Fixed frame: %s", this->fixedFrame.c_str()); stripLeadingSlash(this->fixedFrame, true); this->nodeHandle->get_parameter("sensorFrame", this->sensorFrame); - RCLCPP_DEBUG(this->nodeHandle->get_logger(), "sensor frame: %s", this->sensorFrame.c_str()); stripLeadingSlash(this->sensorFrame, true); this->nodeHandle->get_parameter_or("filteringFrame", this->filteringFrame, this->fixedFrame); - RCLCPP_DEBUG(this->nodeHandle->get_logger(), "filtering frame: %s", this->filteringFrame.c_str()); stripLeadingSlash(this->sensorFrame, true); this->nodeHandle->get_parameter("minDistance", this->minDistance); this->nodeHandle->get_parameter("maxDistance", this->maxDistance); From feeaf3831e2b6bd5de3df28a3c321fb7043beed6 Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 7 May 2024 13:36:50 -0400 Subject: [PATCH 50/54] fixed warnings --- src/RayCastingShapeMask.cpp | 6 +++--- src/TFFramesWatchdog.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/RayCastingShapeMask.cpp b/src/RayCastingShapeMask.cpp index be0a091..ecb9d80 100644 --- a/src/RayCastingShapeMask.cpp +++ b/src/RayCastingShapeMask.cpp @@ -163,7 +163,7 @@ void RayCastingShapeMask::updateBodyPosesNoLock() else { if (containsBody == nullptr){ - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %s without a body", containsHandle); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape with handle %u without a body", containsHandle); } else { std::string name; @@ -172,11 +172,11 @@ void RayCastingShapeMask::updateBodyPosesNoLock() if (name.empty()) { - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s with handle %s", containsBody->getType(), containsHandle); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %u with handle %u", containsBody->getType(), containsHandle); } else { - RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%s)", name.c_str(), containsBody->getType()); + RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%u)", name.c_str(), containsBody->getType()); } } } diff --git a/src/TFFramesWatchdog.cpp b/src/TFFramesWatchdog.cpp index 08ca2c6..a93a807 100644 --- a/src/TFFramesWatchdog.cpp +++ b/src/TFFramesWatchdog.cpp @@ -70,7 +70,7 @@ void TFFramesWatchdog::searchForReachableFrames() RCLCPP_DEBUG(this->nodeHandle->get_logger(), "robot frame: %s, checking against: %s" , this->robotFrame.c_str(), frame.c_str()); if (this->tfBuffer->canTransform(this->robotFrame, frame, time, this->unreachableTfLookupTimeout, &err)) { this->markReachable(frame); - RCLCPP_DEBUG(this->nodeHandle->get_logger(),"TFFramesWatchdog (%s): Frame %s became reachable at %i.%i", + RCLCPP_DEBUG(this->nodeHandle->get_logger(),"TFFramesWatchdog (%s): Frame %s became reachable at %f.%li", this->robotFrame.c_str(), frame.c_str(), time.seconds(), time.nanoseconds()); } else { auto &clk = *nodeHandle->get_clock(); From cc8ec81a01d03d66e74ce2609ad36c7addee0826 Mon Sep 17 00:00:00 2001 From: seb Date: Tue, 7 May 2024 14:09:08 -0400 Subject: [PATCH 51/54] removed removing ros1 remenants --- src/RobotBodyFilter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 4b80424..d8cf638 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -1990,10 +1990,10 @@ void RobotBodyFilter::robotDescriptionUpdated(const std_msgs::msg::String::Sh template bool RobotBodyFilter::triggerModelReload(std_srvs::srv::Trigger_Request&, std_srvs::srv::Trigger_Response&) { std::string urdf; - auto success = this->nodeHandle->getParam(this->robotDescriptionParam, urdf); + auto success = this->nodeHandle->get_parameter(this->robotDescriptionParam, urdf); if (!success) { - ROS_ERROR_STREAM("RobotBodyFilter: Parameter " << this->robotDescriptionParam << " doesn't exist."); + RCLCPP_ERROR(nodeHandle->get_logger(), "RobotBodyFilter: Parameter %s doesn't exist.", this->robotDescriptionParam.c_str()); return false; } From b19a5917bf925bc1832e3d0eea1e24c53c6cbe38 Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Jul 2024 12:29:39 -0400 Subject: [PATCH 52/54] Fixed header inclusion, at least for jazzy & onward --- include/robot_body_filter/utils/tf2_sensor_msgs.h | 2 +- src/RobotBodyFilter.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/robot_body_filter/utils/tf2_sensor_msgs.h b/include/robot_body_filter/utils/tf2_sensor_msgs.h index e13fe76..c0f50e7 100644 --- a/include/robot_body_filter/utils/tf2_sensor_msgs.h +++ b/include/robot_body_filter/utils/tf2_sensor_msgs.h @@ -3,7 +3,7 @@ #include #include -#include +#include namespace robot_body_filter { diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index d8cf638..9a06e28 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include From d0d0cf3db7159a39f49870e6bd0d1d0b21b3a97c Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Wed, 17 Jul 2024 16:39:23 +0000 Subject: [PATCH 53/54] Switched to just the .hpp file rather than the weird double directory include --- include/robot_body_filter/utils/tf2_sensor_msgs.h | 2 +- src/RobotBodyFilter.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/robot_body_filter/utils/tf2_sensor_msgs.h b/include/robot_body_filter/utils/tf2_sensor_msgs.h index c0f50e7..e13fe76 100644 --- a/include/robot_body_filter/utils/tf2_sensor_msgs.h +++ b/include/robot_body_filter/utils/tf2_sensor_msgs.h @@ -3,7 +3,7 @@ #include #include -#include +#include namespace robot_body_filter { diff --git a/src/RobotBodyFilter.cpp b/src/RobotBodyFilter.cpp index 9a06e28..d8cf638 100644 --- a/src/RobotBodyFilter.cpp +++ b/src/RobotBodyFilter.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include From 1b5891221cc8f6d0c8fe2a1bc8845444b9831a7b Mon Sep 17 00:00:00 2001 From: Tyler Mayoff Date: Mon, 29 Jul 2024 10:32:57 -0400 Subject: [PATCH 54/54] Fixed build on jazzy (should also work on humble) --- CMakeLists.txt | 2 +- package.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 85b0cba..438c19c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,7 +11,7 @@ else() add_compile_options(-DROBOT_BODY_FILTER_USE_CXX_OPTIONAL=0) endif() -set(THIS_PACKAGE_DEPS backward_ros urdf urdf_parser_plugin filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen urdf visualization_msgs GLUT ) +set(THIS_PACKAGE_DEPS backward_ros urdf urdf_parser_plugin filters geometric_shapes laser_geometry moveit_core moveit_ros_perception rclcpp sensor_msgs std_srvs pcl_conversions tf2 tf2_ros tf2_eigen tf2_sensor_msgs urdf visualization_msgs GLUT OpenCV) set(MESSAGE_DEPS geometry_msgs std_msgs) find_package(ament_cmake REQUIRED) diff --git a/package.xml b/package.xml index 1721747..f4459c5 100644 --- a/package.xml +++ b/package.xml @@ -35,6 +35,7 @@ pcl_conversions urdf_parser_plugin backward_ros + libopencv-dev tf2_eigen