From aeb5f000e7803265b1bfb6f8ee24b80182f5d9aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Thu, 30 May 2019 00:32:09 +0200 Subject: [PATCH] Port distance_field to ROS 2 (#64) * distance_field, adapt logging https://github.com/ros-planning/moveit2/issues/21 * moveit_core distance field - library as SHARED * Fixing distance_field tests * moveit_core distance field - Fixing logger variable * moveit_core distance_field - converting time to seconds --- moveit_core/distance_field/CMakeLists.txt | 46 ++++-- .../moveit/distance_field/distance_field.h | 37 ++--- .../propagation_distance_field.h | 1 + .../distance_field/src/distance_field.cpp | 53 ++++--- .../src/propagation_distance_field.cpp | 35 ++--- .../test/test_distance_field.cpp | 145 ++++++++++++------ .../distance_field/test/test_voxel_grid.cpp | 2 +- 7 files changed, 192 insertions(+), 127 deletions(-) diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index ac60b52783..4e66fa5a35 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -1,25 +1,47 @@ set(MOVEIT_LIB_NAME moveit_distance_field) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED src/distance_field.cpp src/find_internal_points.cpp src/propagation_distance_field.cpp - ) +) + set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + boost + urdfdom + urdfdom_headers + visualization_msgs + tf2_eigen) + +target_link_libraries(${MOVEIT_LIB_NAME} + ${Boost_LIBRARIES} + ${geometric_shapes_LIBRARIES} +) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_voxel_grid test/test_voxel_grid.cpp) - target_link_libraries(test_voxel_grid ${MOVEIT_LIB_NAME}) + ament_add_gtest(test_voxel_grid test/test_voxel_grid.cpp) + target_link_libraries(test_voxel_grid + ${MOVEIT_LIB_NAME} + ) + + ament_add_gtest(test_distance_field test/test_distance_field.cpp) - catkin_add_gtest(test_distance_field test/test_distance_field.cpp) - target_link_libraries(test_distance_field ${MOVEIT_LIB_NAME}) + target_link_libraries(test_distance_field + ${MOVEIT_LIB_NAME} + ${visualization_msgs_LIBRARIES} + ${tf2_eigen_LIBRARIES} + ${Boost_LIBRARIES} + ${geometric_shapes_LIBRARIES} + ${OCTOMAP_LIBRARIES} + ${console_bridge_LIBRARIES} + ) endif() diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h index d1164a4935..3e6faacf6f 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/distance_field.h @@ -42,12 +42,13 @@ #include #include #include -#include -#include +#include +#include #include #include #include #include +#include "rclcpp/rclcpp.hpp" namespace shapes { @@ -192,7 +193,7 @@ class DistanceField void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); // DEPRECATED form - MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); + MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose); /** * \brief Adds an octree to the distance field. Cells that are @@ -231,8 +232,8 @@ class DistanceField const Eigen::Isometry3d& new_pose); // DEPRECATED form - MOVEIT_DEPRECATED void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose, - const geometry_msgs::Pose& new_pose); + MOVEIT_DEPRECATED void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& old_pose, + const geometry_msgs::msg::Pose& new_pose); /** * \brief All points corresponding to the shape are removed from the @@ -246,7 +247,7 @@ class DistanceField void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose); // DEPRECATED form - MOVEIT_DEPRECATED void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose); + MOVEIT_DEPRECATED void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose); /** * \brief Resets all points in the distance field to an uninitialize @@ -430,7 +431,7 @@ class DistanceField * \brief Get an iso-surface for visualization in rviz. The * iso-surface shows every cell that has a distance in a given * range in the distance field. The cells are added as a - * visualization_msgs::Marker::CUBE_LIST in the namespace + * visualization_msgs::msg::Marker::CUBE_LIST in the namespace * "distance_field". * * @param [in] min_distance Cells of less than this distance will not be added to the marker @@ -440,13 +441,13 @@ class DistanceField * @param [out] marker The marker that will contain the indicated cells. */ void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id, - const ros::Time stamp, visualization_msgs::Marker& marker) const; + const rclcpp::Time stamp, visualization_msgs::msg::Marker& marker) const; /** * \brief Populates the supplied marker array with a series of * arrows representing gradients of cells that are within the * supplied range in terms of distance. The markers will be - * visualization_msgs::Marker::ARROW in the namespace + * visualization_msgs::msg::Marker::ARROW in the namespace * "distance_field_gradient". * * @param [in] min_distance Cells of less than this distance will not be added to the marker @@ -455,8 +456,8 @@ class DistanceField * @param [in] stamp The stamp to use in the header of the marker * @param [out] marker_array The marker array to populate */ - void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const ros::Time& stamp, - visualization_msgs::MarkerArray& marker_array) const; + void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const rclcpp::Time& stamp, + visualization_msgs::msg::MarkerArray& marker_array) const; /** * \brief Populates a marker with a slice of the distance field in a @@ -484,8 +485,8 @@ class DistanceField * @param [out] marker The marker that will contain the indicated cells. */ void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, - const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp, - visualization_msgs::Marker& marker) const; + const Eigen::Vector3d& origin, const std::string& frame_id, const rclcpp::Time stamp, + visualization_msgs::msg::Marker& marker) const; /** * \brief A function that populates the marker with three planes - * one each along the XY, XZ, and YZ axes. For each of the planes, @@ -500,10 +501,10 @@ class DistanceField * the marker. * * @param [out] marker The marker, which will be populated with a - * visualization_msgs::Marker::CUBE_LIST . + * visualization_msgs::msg::Marker::CUBE_LIST . */ - void getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_distance, - visualization_msgs::Marker& marker) const; + void getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_distance, + visualization_msgs::msg::Marker& marker) const; /** * \brief Gets the distance field size along the X dimension in meters @@ -613,8 +614,8 @@ class DistanceField * * @param [in] max_distance The distance past which all cells will be fully white */ - void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point, std_msgs::ColorRGBA& color, - double max_distance) const; + void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point, + std_msgs::msg::ColorRGBA& color, double max_distance) const; double size_x_; /**< \brief X size of the distance field */ double size_y_; /**< \brief Y size of the distance field */ diff --git a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h index 66cbf2daa2..92cd5cf69e 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h +++ b/moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h @@ -44,6 +44,7 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace EigenSTL { diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 071777706b..4437ed3cac 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -38,12 +38,14 @@ #include #include #include -#include #include #include +#include "rclcpp/rclcpp.hpp" namespace distance_field { +rclcpp::Logger LOGGER_DISTANCE_FIELD = rclcpp::get_logger("moveit").get_child("distance_field"); + DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z) : size_x_(size_x) @@ -86,15 +88,15 @@ double DistanceField::getDistanceGradient(double x, double y, double z, double& } void DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id, - const ros::Time stamp, visualization_msgs::Marker& inf_marker) const + const rclcpp::Time stamp, visualization_msgs::msg::Marker& inf_marker) const { inf_marker.points.clear(); inf_marker.header.frame_id = frame_id; inf_marker.header.stamp = stamp; inf_marker.ns = "distance_field"; inf_marker.id = 1; - inf_marker.type = visualization_msgs::Marker::CUBE_LIST; - inf_marker.action = visualization_msgs::Marker::MODIFY; + inf_marker.type = visualization_msgs::msg::Marker::CUBE_LIST; + inf_marker.action = visualization_msgs::msg::Marker::MODIFY; inf_marker.scale.x = resolution_; inf_marker.scale.y = resolution_; inf_marker.scale.z = resolution_; @@ -130,7 +132,8 @@ void DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distanc } void DistanceField::getGradientMarkers(double min_distance, double max_distance, const std::string& frame_id, - const ros::Time& stamp, visualization_msgs::MarkerArray& marker_array) const + const rclcpp::Time& stamp, + visualization_msgs::msg::MarkerArray& marker_array) const { Eigen::Vector3d unit_x(1, 0, 0); Eigen::Vector3d unit_y(0, 1, 0); @@ -154,15 +157,15 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance, if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0) { - visualization_msgs::Marker marker; + visualization_msgs::msg::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = stamp; marker.ns = "distance_field_gradient"; marker.id = id++; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; marker.pose.position.x = world_x; marker.pose.position.y = world_y; @@ -201,7 +204,7 @@ bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isom const shapes::OcTree* oc = dynamic_cast(shape); if (!oc) { - ROS_ERROR_NAMED("distance_field", "Problem dynamic casting shape that claims to be OcTree"); + RCLCPP_ERROR(LOGGER_DISTANCE_FIELD, "Problem dynamic casting shape that claims to be OcTree"); return false; } getOcTreePoints(oc->octree.get(), points); @@ -224,7 +227,7 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Iso } // DEPRECATED -void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose) +void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose) { Eigen::Isometry3d pose_e; tf2::fromMsg(pose, pose_e); @@ -289,7 +292,7 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is { if (shape->type == shapes::OCTREE) { - ROS_WARN_NAMED("distance_field", "Move shape not supported for Octree"); + RCLCPP_WARN(LOGGER_DISTANCE_FIELD, "Move shape not supported for Octree"); return; } bodies::Body* body = bodies::createBodyFromShape(shape); @@ -304,8 +307,8 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is } // DEPRECATED -void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose, - const geometry_msgs::Pose& new_pose) +void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& old_pose, + const geometry_msgs::msg::Pose& new_pose) { Eigen::Isometry3d old_pose_e, new_pose_e; tf2::fromMsg(old_pose, old_pose_e); @@ -324,7 +327,7 @@ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen } // DEPRECATED -void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose) +void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose) { Eigen::Isometry3d pose_e; tf2::fromMsg(pose, pose_e); @@ -332,15 +335,15 @@ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geome } void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, - const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp, - visualization_msgs::Marker& plane_marker) const + const Eigen::Vector3d& origin, const std::string& frame_id, + const rclcpp::Time stamp, visualization_msgs::msg::Marker& plane_marker) const { plane_marker.header.frame_id = frame_id; plane_marker.header.stamp = stamp; plane_marker.ns = "distance_field_plane"; plane_marker.id = 1; - plane_marker.type = visualization_msgs::Marker::CUBE_LIST; - plane_marker.action = visualization_msgs::Marker::ADD; + plane_marker.type = visualization_msgs::msg::Marker::CUBE_LIST; + plane_marker.action = visualization_msgs::msg::Marker::ADD; plane_marker.scale.x = resolution_; plane_marker.scale.y = resolution_; plane_marker.scale.z = resolution_; @@ -442,8 +445,8 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, } } -void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point, - std_msgs::ColorRGBA& color, double max_distance) const +void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point, + std_msgs::msg::ColorRGBA& color, double max_distance) const { double wx, wy, wz; gridToWorld(xCell, yCell, zCell, wx, wy, wz); @@ -457,8 +460,8 @@ void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geome color.b = dist / max_distance; // dist/max_distance * 0.1; } -void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_dist, - visualization_msgs::Marker& marker) const +void DistanceField::getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_dist, + visualization_msgs::msg::Marker& marker) const { int max_x_cell = getXNumCells(); int max_y_cell = getYNumCells(); @@ -503,8 +506,8 @@ void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros:: marker.header.stamp = stamp; marker.ns = "distance_field_projection_plane"; marker.id = 1; - marker.type = visualization_msgs::Marker::CUBE_LIST; - marker.action = visualization_msgs::Marker::MODIFY; + marker.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker.action = visualization_msgs::msg::Marker::MODIFY; marker.scale.x = getResolution(); marker.scale.y = getResolution(); marker.scale.z = getResolution(); @@ -557,4 +560,4 @@ void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros:: delete[] z_projection; } -} // end of namespace distance_field \ No newline at end of file +} // end of namespace distance_field diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index ae3efdfe99..9ac8d299f8 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -35,14 +35,16 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson */ #include -#include -#include +#include #include #include #include +#include "rclcpp/rclcpp.hpp" namespace distance_field { +rclcpp::Logger LOGGER_PROPAGATION_DISTANCE_FIELD = rclcpp::get_logger("moveit").get_child("distance_field"); + PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, double max_distance, bool propagate_negative) @@ -102,26 +104,26 @@ int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i void PropagationDistanceField::print(const VoxelSet& set) { - ROS_DEBUG_NAMED("distance_field", "["); + RCLCPP_DEBUG(LOGGER_PROPAGATION_DISTANCE_FIELD, "["); VoxelSet::const_iterator it; for (it = set.begin(); it != set.end(); ++it) { Eigen::Vector3i loc1 = *it; - ROS_DEBUG_NAMED("distance_field", "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z()); + RCLCPP_DEBUG(LOGGER_PROPAGATION_DISTANCE_FIELD, "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z()); } - ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)set.size()); + RCLCPP_DEBUG(LOGGER_PROPAGATION_DISTANCE_FIELD, "] size=%u\n", (unsigned int)set.size()); } void PropagationDistanceField::print(const EigenSTL::vector_Vector3d& points) { - ROS_DEBUG_NAMED("distance_field", "["); + RCLCPP_DEBUG(LOGGER_PROPAGATION_DISTANCE_FIELD, "["); EigenSTL::vector_Vector3d::const_iterator it; for (it = points.begin(); it != points.end(); ++it) { Eigen::Vector3d loc1 = *it; - ROS_DEBUG_NAMED("distance_field", "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z()); + RCLCPP_DEBUG(LOGGER_PROPAGATION_DISTANCE_FIELD, "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z()); } - ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)points.size()); + RCLCPP_DEBUG(LOGGER_PROPAGATION_DISTANCE_FIELD, "] size=%u\n", (unsigned int)points.size()); } void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector3d& old_points, @@ -167,19 +169,10 @@ void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector { new_not_in_current.push_back(new_not_old[i]); } - // ROS_INFO_NAMED("distance_field", "Adding obstacle voxel %d %d %d", (*it).x(), (*it).y(), (*it).z()); } removeObstacleVoxels(old_not_new); addNewObstacleVoxels(new_not_in_current); - - // ROS_DEBUG_NAMED("distance_field", "new=" ); - // print(points_added); - // ROS_DEBUG_NAMED("distance_field", "removed=" ); - // print(points_removed); - // ROS_DEBUG_NAMED("distance_field", "obstacle_voxel_locations_=" ); - // print(object_voxel_locations_); - // ROS_DEBUG_NAMED("distance_field", ""); } void PropagationDistanceField::addPointsToField(const EigenSTL::vector_Vector3d& points) @@ -416,8 +409,8 @@ void PropagationDistanceField::propagatePositive() // This will never happen. update_direction_ is always set before voxel is added to bucket queue. if (vptr->update_direction_ < 0 || vptr->update_direction_ > 26) { - ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d", - vptr->update_direction_); + RCLCPP_ERROR(LOGGER_PROPAGATION_DISTANCE_FIELD, "PROGRAMMING ERROR: Invalid update direction detected: %d", + vptr->update_direction_); continue; } @@ -475,8 +468,8 @@ void PropagationDistanceField::propagateNegative() // negative_bucket_queue_. if (vptr->negative_update_direction_ < 0 || vptr->negative_update_direction_ > 26) { - ROS_ERROR_NAMED("distance_field", "PROGRAMMING ERROR: Invalid update direction detected: %d", - vptr->update_direction_); + RCLCPP_ERROR(LOGGER_PROPAGATION_DISTANCE_FIELD, "PROGRAMMING ERROR: Invalid update direction detected: %d", + vptr->update_direction_); continue; } diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index 7417915298..bb2de330c4 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -42,8 +42,6 @@ #include #include #include -#include - #include using namespace distance_field; @@ -88,7 +86,7 @@ void print(PropagationDistanceField& pdf, int numX, int numY, int numZ) { if (pdf.getCell(x, y, z).distance_square_ == 0) { - // ROS_INFO_NAMED("distance_field", "Obstacle cell %d %d %d", x, y, z); + // RCLCPP_INFO("distance_field", "Obstacle cell %d %d %d", x, y, z); } } } @@ -350,7 +348,7 @@ TEST(TestPropagationDistanceField, TestAddRemovePoints) EigenSTL::vector_Vector3d points; points.push_back(POINT1); points.push_back(POINT2); - ROS_INFO_NAMED("distance_field", "Adding %zu points", points.size()); + // RCLCPP_INFO("distance_field", "Adding %zu points", points.size()); df.addPointsToField(points); // print(df, numX, numY, numZ); @@ -466,7 +464,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) } df.reset(); - ROS_INFO_NAMED("distance_field", "Adding %zu points", points.size()); + // RCLCPP_INFO("distance_field", "Adding %zu points", points.size()); df.addPointsToField(points); // print(df, numX, numY, numZ); // printNeg(df, numX, numY, numZ); @@ -501,7 +499,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints) shapes::Sphere sphere(.25); - geometry_msgs::Pose p; + geometry_msgs::msg::Pose p; p.orientation.w = 1.0; p.position.x = .5; p.position.y = .5; @@ -664,16 +662,22 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) << (PERF_WIDTH / PERF_RESOLUTION) * (PERF_HEIGHT / PERF_RESOLUTION) * (PERF_DEPTH / PERF_RESOLUTION) << " entries" << std::endl; - ros::WallTime dt = ros::WallTime::now(); + auto dt = std::chrono::system_clock::now(); PropagationDistanceField df(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION, PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, false); - std::cout << "Creating unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Creating unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); PropagationDistanceField sdf(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION, PERF_ORIGIN_X, PERF_ORIGIN_Y, PERF_ORIGIN_Z, PERF_MAX_DIST, true); - std::cout << "Creating signed took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Creating signed took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; shapes::Box big_table(2.0, 2.0, .5); @@ -686,37 +690,66 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) std::cout << "Adding " << big_num_points << " points" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); df.addShapeToField(&big_table, p); - std::cout << "Adding to unsigned took " << (ros::WallTime::now() - dt).toSec() << " avg " - << (ros::WallTime::now() - dt).toSec() / (big_num_points * 1.0) << std::endl; - - dt = ros::WallTime::now(); + std::cout << "Adding to unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" + << " avg " + << (std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0) / + (big_num_points * 1.0) + << std::endl; + + dt = std::chrono::system_clock::now(); df.addShapeToField(&big_table, p); - std::cout << "Re-adding to unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Re-adding to unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); sdf.addShapeToField(&big_table, p); - std::cout << "Adding to signed took " << (ros::WallTime::now() - dt).toSec() << " avg " - << (ros::WallTime::now() - dt).toSec() / (big_num_points * 1.0) << std::endl; - - dt = ros::WallTime::now(); + std::cout << "Adding to signed took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" + << " avg " + << (std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0) / + (big_num_points * 1.0) + << std::endl; + + dt = std::chrono::system_clock::now(); df.moveShapeInField(&big_table, p, np); - std::cout << "Moving in unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Moving in unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); sdf.moveShapeInField(&big_table, p, np); - std::cout << "Moving in signed took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Moving in signed took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); df.removeShapeFromField(&big_table, np); - std::cout << "Removing from unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Removing from unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); sdf.removeShapeFromField(&big_table, np); - std::cout << "Removing from signed took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Removing from signed took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); df.reset(); shapes::Box small_table(.25, .25, .05); @@ -725,23 +758,35 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) std::cout << "Adding " << small_num_points << " points" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); df.addShapeToField(&small_table, p); - std::cout << "Adding to unsigned took " << (ros::WallTime::now() - dt).toSec() << " avg " - << (ros::WallTime::now() - dt).toSec() / (small_num_points * 1.0) << std::endl; + std::cout << "Adding to unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" + << " avg " << (std::chrono::system_clock::now() - dt).count() / (small_num_points * 1.0) << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); sdf.addShapeToField(&small_table, p); - std::cout << "Adding to signed took " << (ros::WallTime::now() - dt).toSec() << " avg " - << (ros::WallTime::now() - dt).toSec() / (small_num_points * 1.0) << std::endl; + std::cout << "Adding to signed took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" + << " avg " << (std::chrono::system_clock::now() - dt).count() / (small_num_points * 1.0) << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); df.moveShapeInField(&small_table, p, np); - std::cout << "Moving in unsigned took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Moving in unsigned took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); sdf.moveShapeInField(&small_table, p, np); - std::cout << "Moving in signed took " << (ros::WallTime::now() - dt).toSec() << std::endl; + std::cout << "Moving in signed took " + << std::chrono::duration_cast(std::chrono::system_clock::now() - dt).count() / + 1000.0 + << " secs" << std::endl; // uniformly spaced points - a worst case scenario PropagationDistanceField worstdfu(PERF_WIDTH, PERF_HEIGHT, PERF_DEPTH, PERF_RESOLUTION, PERF_ORIGIN_X, PERF_ORIGIN_Y, @@ -764,7 +809,7 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) if (!valid) { - ROS_WARN_NAMED("distance_field", "Something wrong"); + // RCLCPP_WARN("distance_field", "Something wrong"); continue; } bad_vec.push_back(loc); @@ -772,16 +817,16 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) } } - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); worstdfu.addPointsToField(bad_vec); - ros::WallDuration wd = ros::WallTime::now() - dt; - printf("Time for unsigned adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.toSec(), - wd.toSec() / (bad_vec.size() * 1.0)); - dt = ros::WallTime::now(); + auto wd = std::chrono::system_clock::now() - dt; + printf("Time for unsigned adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.count(), + wd.count() / (bad_vec.size() * 1.0)); + dt = std::chrono::system_clock::now(); worstdfs.addPointsToField(bad_vec); - wd = ros::WallTime::now() - dt; - printf("Time for signed adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.toSec(), - wd.toSec() / (bad_vec.size() * 1.0)); + wd = std::chrono::system_clock::now() - dt; + printf("Time for signed adding %u uniform points is %g average %g\n", (unsigned int)bad_vec.size(), wd.count(), + wd.count() / (bad_vec.size() * 1.0)); } TEST(TestSignedPropagationDistanceField, TestOcTree) @@ -923,9 +968,9 @@ TEST(TestSignedPropagationDistanceField, TestReadWrite) PropagationDistanceField dfx(i, PERF_MAX_DIST, false); std::ifstream i2("test_big.df", std::ios::in); - ros::WallTime wt = ros::WallTime::now(); + auto wt = std::chrono::system_clock::now(); PropagationDistanceField df3(i2, PERF_MAX_DIST + .02, false); - std::cout << "Reconstruction for big file took " << (ros::WallTime::now() - wt).toSec() << std::endl; + std::cout << "Reconstruction for big file took " << (std::chrono::system_clock::now() - wt).count() << std::endl; EXPECT_FALSE(areDistanceFieldsDistancesEqual(df, df3)); } diff --git a/moveit_core/distance_field/test/test_voxel_grid.cpp b/moveit_core/distance_field/test/test_voxel_grid.cpp index 7538583615..c1e30525f8 100644 --- a/moveit_core/distance_field/test/test_voxel_grid.cpp +++ b/moveit_core/distance_field/test/test_voxel_grid.cpp @@ -37,7 +37,7 @@ #include #include -#include +#include "rclcpp/rclcpp.hpp" using namespace distance_field;