Merge pull request #18 from Guidebolt/body-filter-debugging

obb commented out, tested on humble

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 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)
@@ -28,23 +28,25 @@ 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
-  # 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})
 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
+$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+$<INSTALL_INTERFACE:include>)
 target_link_libraries(${PROJECT_NAME}
   ${PROJECT_NAME}_utils
   tf2_sensor_msgs_rbf
@@ -61,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 ${PROJECT_NAME}_utils
+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
@@ -132,5 +138,10 @@ if (BUILD_TESTING)
     DESTINATION share/robot_body_filter/test)
 endif()
 
-ament_export_dependencies(${THIS_PACKAGE_DEPS})
+ament_export_include_directories(include include/${PROJECT_NAME} include/${PROJECT_NAME}/utils)
+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}
+)
 ament_package()

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

include/robot_body_filter/RobotBodyFilter.h

@@ -8,6 +8,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 // #include <robot_body_filter/utils/filter_utils.hpp>
+// #include <robot_body_filter/robot_body_filter/msg/oriented_bounding_box_stamped.hpp>
 #include <robot_body_filter/utils/tf2_sensor_msgs.h>
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <robot_body_filter/RayCastingShapeMask.h>
@@ -100,9 +101,10 @@ class RobotBodyFilter : public filters::FilterBase<T> {
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
-  RobotBodyFilter();
+  RobotBodyFilter(std::shared_ptr<rclcpp::Node> inputNode = std::make_shared<rclcpp::Node>("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;
@@ -204,7 +206,9 @@ class RobotBodyFilter : public filters::FilterBase<T> {
   std::string robotDescriptionParam;
 
   //! Subscriber for robot_description updates.
-  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robotDescriptionUpdatesListener;
+  std::shared_ptr<rclcpp::AsyncParametersClient> 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;
@@ -322,7 +326,7 @@ class RobotBodyFilter : public filters::FilterBase<T> {
   //! tf client
   std::shared_ptr<tf2_ros::Buffer> tfBuffer;
   //! tf listener
-  std::unique_ptr<tf2_ros::TransformListener> tfListener;
+  std::shared_ptr<tf2_ros::TransformListener> tfListener;
 
   //! Watchdog for unreachable frames.
   std::shared_ptr<TFFramesWatchdog> tfFramesWatchdog;
@@ -462,6 +466,7 @@ class RobotBodyFilter : public filters::FilterBase<T> {
 class RobotBodyFilterLaserScan : public RobotBodyFilter<sensor_msgs::msg::LaserScan>
 {
 public:
+  void DeclareParameters();
   //! Apply the filter.
   bool update(const sensor_msgs::msg::LaserScan& inputScan, sensor_msgs::msg::LaserScan& filteredScan) override;
 
@@ -477,6 +482,7 @@ class RobotBodyFilterLaserScan : public RobotBodyFilter<sensor_msgs::msg::LaserS
 class RobotBodyFilterPointCloud2 : public RobotBodyFilter<sensor_msgs::msg::PointCloud2>
 {
 public:
+  void DeclareParameters();
   //! Apply the filter.
   bool update(const sensor_msgs::msg::PointCloud2& inputCloud, sensor_msgs::msg::PointCloud2& filteredCloud) override;
 

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);

include/robot_body_filter/utils/string_utils.hpp

@@ -126,7 +126,7 @@ inline std::string to_string(const std::vector<T> &value)
 }
 
 template<typename T>
-inline std::string to_string(const std::set<T> &value)
+inline std::string to_string(std::set<T> &value)
 {
   std::stringstream ss;
   ss << "[";
@@ -142,7 +142,8 @@ inline std::string to_string(const std::set<T> &value)
     ++i;
   }
   ss << "]";
-  return ss.str();
+  auto string = ss.str();
+  return string;
 }
 
 template<typename K, typename V>

include/robot_body_filter/utils/tf2_sensor_msgs.h

@@ -3,7 +3,7 @@
 
 #include <unordered_map>
 #include <tf2_ros/buffer.h>
-#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
 
 namespace robot_body_filter
 {

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);
 
 };
 

package.xml

@@ -33,6 +33,8 @@
   <depend>urdf</depend>
   <depend>visualization_msgs</depend>
   <depend>pcl_conversions</depend>
+  <depend>urdf_parser_plugin</depend>
+  <depend>backward_ros</depend>
   <!-- <depend>fcl</depend> -->
 
   <build_depend>tf2_eigen</build_depend>

src/RayCastingShapeMask.cpp

@@ -1,5 +1,7 @@
 /* HACK HACK HACK */
 /* We want to subclass ShapeMask and use its private members. */
+#include <rclcpp/logger.hpp>
+#include <rclcpp/logging.hpp>
 #include <sstream>  // has to be there, otherwise we encounter build problems
 #define private protected
 #include <moveit/point_containment_filter/shape_mask.h>
@@ -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 %u 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 %u with handle %u", containsBody->getType(), containsHandle);
+        }
+        else
+        {
+          RCLCPP_ERROR(rclcpp::get_logger("robot_body_filter"),"shape_mask Missing transform for shape %s (%u)", name.c_str(), containsBody->getType());
+        }
       }
     }
   }