From 4d0dca11e2e2dc9c102f9e53a7814501a7748313 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 31 Mar 2019 18:17:32 +0200 Subject: [PATCH] Port distance_field to ROS 2 --- moveit_core/distance_field/CMakeLists.txt | 32 ++++--- .../moveit/distance_field/distance_field.h | 35 ++++---- .../propagation_distance_field.h | 1 + .../distance_field/src/distance_field.cpp | 51 +++++------ .../src/propagation_distance_field.cpp | 31 +++---- .../test/test_distance_field.cpp | 86 +++++++++---------- .../distance_field/test/test_voxel_grid.cpp | 2 +- 7 files changed, 123 insertions(+), 115 deletions(-) diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index ac60b52783..87b39dd14f 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -7,19 +7,25 @@ add_library(${MOVEIT_LIB_NAME} ) 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} + urdfdom + urdfdom_headers + visualization_msgs + tf2_eigen) 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(CATKIN_ENABLE_TESTING) - catkin_add_gtest(test_voxel_grid test/test_voxel_grid.cpp) - target_link_libraries(test_voxel_grid ${MOVEIT_LIB_NAME}) - - catkin_add_gtest(test_distance_field test/test_distance_field.cpp) - target_link_libraries(test_distance_field ${MOVEIT_LIB_NAME}) -endif() +# if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) +# ament_add_gtest(test_voxel_grid test/test_voxel_grid.cpp) +# ament_target_dependencies(test_voxel_grid +# ${MOVEIT_LIB_NAME}) +# ament_add_gtest(test_distance_field test/test_distance_field.cpp) +# ament_target_dependencies(test_distance_field +# ${MOVEIT_LIB_NAME} +# visualization_msgs +# tf2_eigen) +# 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..9f883a4712 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,7 +614,7 @@ 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, + 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 */ 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..17f910e6c8 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -38,10 +38,11 @@ #include #include #include -#include #include #include +rclcpp::Logger logger = rclcpp::get_logger("distance_field"); + namespace distance_field { DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, @@ -86,15 +87,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 +131,7 @@ 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 +155,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 +202,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, "Problem dynamic casting shape that claims to be OcTree"); return false; } getOcTreePoints(oc->octree.get(), points); @@ -224,7 +225,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 +290,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, "Move shape not supported for Octree"); return; } bodies::Body* body = bodies::createBodyFromShape(shape); @@ -304,8 +305,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 +325,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 +333,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 +443,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 +458,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 +504,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 +558,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..7b5a2093b5 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -35,12 +35,13 @@ /* Author: Mrinal Kalakrishnan, Ken Anderson */ #include -#include -#include +#include #include #include #include +rclcpp::Logger logger = rclcpp::get_logger("distance_field"); + namespace distance_field { PropagationDistanceField::PropagationDistanceField(double size_x, double size_y, double size_z, double resolution, @@ -102,26 +103,26 @@ int PropagationDistanceField::eucDistSq(Eigen::Vector3i point1, Eigen::Vector3i void PropagationDistanceField::print(const VoxelSet& set) { - ROS_DEBUG_NAMED("distance_field", "["); + RCLCPP_DEBUG(logger, "["); 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, "%d, %d, %d ", loc1.x(), loc1.y(), loc1.z()); } - ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)set.size()); + RCLCPP_DEBUG(logger, "] size=%u\n", (unsigned int)set.size()); } void PropagationDistanceField::print(const EigenSTL::vector_Vector3d& points) { - ROS_DEBUG_NAMED("distance_field", "["); + RCLCPP_DEBUG(logger, "["); 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, "%g, %g, %g ", loc1.x(), loc1.y(), loc1.z()); } - ROS_DEBUG_NAMED("distance_field", "] size=%u\n", (unsigned int)points.size()); + RCLCPP_DEBUG(logger, "] size=%u\n", (unsigned int)points.size()); } void PropagationDistanceField::updatePointsInField(const EigenSTL::vector_Vector3d& old_points, @@ -167,19 +168,19 @@ 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()); + // RCLCPP_INFO(logger, "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=" ); + // RCLCPP_DEBUG(logger, "new=" ); // print(points_added); - // ROS_DEBUG_NAMED("distance_field", "removed=" ); + // RCLCPP_DEBUG(logger, "removed=" ); // print(points_removed); - // ROS_DEBUG_NAMED("distance_field", "obstacle_voxel_locations_=" ); + // RCLCPP_DEBUG(logger, "obstacle_voxel_locations_=" ); // print(object_voxel_locations_); - // ROS_DEBUG_NAMED("distance_field", ""); + // RCLCPP_DEBUG(logger, ""); } void PropagationDistanceField::addPointsToField(const EigenSTL::vector_Vector3d& points) @@ -416,7 +417,7 @@ 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", + RCLCPP_ERROR(logger, "PROGRAMMING ERROR: Invalid update direction detected: %d", vptr->update_direction_); continue; } @@ -475,7 +476,7 @@ 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", + RCLCPP_ERROR(logger, "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..a916d9287e 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,16 @@ 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::system_clock::now() - dt).toSec() << 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::system_clock::now() - dt).toSec() << std::endl; shapes::Box big_table(2.0, 2.0, .5); @@ -686,37 +684,37 @@ 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; + std::cout << "Adding to unsigned took " << (std::chrono::system_clock::now() - dt).toSec() << " avg " + << (std::chrono::system_clock::now() - dt).toSec() / (big_num_points * 1.0) << std::endl; - dt = ros::WallTime::now(); + 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::system_clock::now() - dt).toSec() << 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; + std::cout << "Adding to signed took " << (std::chrono::system_clock::now() - dt).toSec() << " avg " + << (std::chrono::system_clock::now() - dt).toSec() / (big_num_points * 1.0) << std::endl; - dt = ros::WallTime::now(); + 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::system_clock::now() - dt).toSec() << 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::system_clock::now() - dt).toSec() << 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::system_clock::now() - dt).toSec() << 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::system_clock::now() - dt).toSec() << std::endl; - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); df.reset(); shapes::Box small_table(.25, .25, .05); @@ -725,23 +723,23 @@ 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::system_clock::now() - dt).toSec() << " avg " + << (std::chrono::system_clock::now() - dt).toSec() / (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::system_clock::now() - dt).toSec() << " avg " + << (std::chrono::system_clock::now() - dt).toSec() / (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::system_clock::now() - dt).toSec() << 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::system_clock::now() - dt).toSec() << 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 +762,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,14 +770,14 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) } } - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); worstdfu.addPointsToField(bad_vec); - ros::WallDuration wd = ros::WallTime::now() - dt; + ros::WallDuration 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.toSec(), wd.toSec() / (bad_vec.size() * 1.0)); - dt = ros::WallTime::now(); + dt = std::chrono::system_clock::now(); worstdfs.addPointsToField(bad_vec); - wd = ros::WallTime::now() - dt; + 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.toSec(), wd.toSec() / (bad_vec.size() * 1.0)); } @@ -923,9 +921,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(); + ros::WallTime 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).toSec() << 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;