diff --git a/plane_seg/CMakeLists.txt b/plane_seg/CMakeLists.txt index b0488c2..a926b91 100644 --- a/plane_seg/CMakeLists.txt +++ b/plane_seg/CMakeLists.txt @@ -40,9 +40,9 @@ target_link_libraries(plane_seg ${catkin_LIBRARIES}) # standalone lcm-based block fitter -set(APP_NAME block_fitter) -add_executable(${APP_NAME} src/block-fitter.cpp) -target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES}) +#set(APP_NAME block_fitter) +#add_executable(${APP_NAME} src/block-fitter.cpp) +#target_link_libraries(${APP_NAME} boost_system ${catkin_LIBRARIES}) # test program @@ -65,3 +65,4 @@ install(TARGETS plane_seg # Mark header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + diff --git a/terrain_simplification/CMakeLists.txt b/terrain_simplification/CMakeLists.txt new file mode 100644 index 0000000..76a0fb3 --- /dev/null +++ b/terrain_simplification/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 2.8.3) +project(terrain_simplification) + +add_compile_options(-std=c++11) +set(DEFAULT_BUILD "Release") +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to '${DEFAULT_BUILD}' as none was specified.") + set(CMAKE_BUILD_TYPE ${DEFAULT_BUILD} CACHE STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" + "Release" + "MinSizeRel" + "RelWithDebInfo") +endif() + +add_definitions(-DMELO_FUNCTION_PRINTS) + +find_package(catkin REQUIRED COMPONENTS + grid_map_core + grid_map_filters + grid_map_cv + ) + +find_package(OpenCV REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + grid_map_core + grid_map_filters + grid_map_cv + ) + +########### +## Build ## +########### +add_library(${PROJECT_NAME} + src/terrain_simplification.cpp + ) + +include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ) + +add_dependencies(${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${OpenCV_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS}) + + +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBS} + ${catkin_LIBRARIES} + ) + +############# +## Install ## +############# +# Mark library for installation +install( + TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +# Mark header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.hpp" + ) diff --git a/terrain_simplification/README.md b/terrain_simplification/README.md new file mode 100644 index 0000000..4ed6418 --- /dev/null +++ b/terrain_simplification/README.md @@ -0,0 +1,7 @@ +![A gif demonstrating the use of terrain simplification as a base-motion-constraint, shown at 5x speed.](https://github.com/ori-drs/plane_seg/blob/add-terrain-simplification/terrain_simplification/terrain-simplification_base-motion-constraint_5x-speed.gif) + +![A gif demonstrating the use of terrain simplification in simulation.](https://github.com/ori-drs/plane_seg/blob/add-terrain-simplification/terrain_simplification/terrain-simplification_base-motion-constraint_simulation.gif) + +![An image demonstrating the use of terrain simplification to compute first- and second-order partial derivatives of any image.](https://github.com/ori-drs/plane_seg/blob/add-terrain-simplification/terrain_simplification/terrain-simplification_derivatives.png) + +![A traversability map computed using FilterChain.](https://github.com/ori-drs/plane_seg/blob/add-terrain-simplification/terrain_simplification/terrain-simplification_traversability.png) diff --git a/terrain_simplification/include/terrain_simplification/terrain_simplification.hpp b/terrain_simplification/include/terrain_simplification/terrain_simplification.hpp new file mode 100644 index 0000000..4bcfc67 --- /dev/null +++ b/terrain_simplification/include/terrain_simplification/terrain_simplification.hpp @@ -0,0 +1,422 @@ +/** + * @file terrain_simplification.hpp + * @brief A module that simplifies the observed terrain, represented as a gridmap, + * by applying an OpenCV-based low-pass filter to extract the high-level geometry + * and qualify its irregularity by computing traversability using a filter chain. + * @author Oliwier Melon (omelon@robots.ox.ac.uk) + * @bug No known bugs. + * @date 07/01/2021 + * @version 1.1 + * @note The computation can be accelerated if the functionality of the Filter Chain + * were manually implemented. + * @copyright 2020, Oliwier Melon. BSD-3-Clause + */ + +#ifndef TERRAIN_SIMPLIFICATION_HPP +#define TERRAIN_SIMPLIFICATION_HPP + +#include +#include + +#if defined(__GNUG__) || defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#endif + +#include + +#if defined(__GNUG__) || defined(__clang__) +#pragma GCC diagnostic pop +#endif + +#include +#include +#include + +namespace terrain_simplification { + +enum Dim { X=0, Y, XY }; + +struct D { + cv::Mat d; + cv::Mat dx; + cv::Mat dy; +}; + +struct DD { + cv::Mat dd; + cv::Mat dxdx; + cv::Mat dxdy; + cv::Mat dydx; + cv::Mat dydy; +}; + +struct MD { + cv::Mat m; + D f; +}; + +struct MDD { + cv::Mat m; + D f; + DD s; +}; + +class TerrainSimplification { + +public: + /** + * @brief Constructs a multi-threaded (2) object + */ + TerrainSimplification(); + + /** + * @brief Destructs the multi-threaded (2) object + */ + virtual ~TerrainSimplification(); + + /** + * @brief Assigns a shared pointer to the filter_chain_ object + * @param[in] filter_chain shared pointer to a filter chain object in the derived class (ros). + */ + void setFilterChain( + std::shared_ptr > filter_chain) { + filter_chain_ = filter_chain; + } + + /** + * @brief Set the position and orientation of the robot + * @param[in] position the position of the robot + * @param[in] orientation the orientation of the robot + */ + virtual void setRobotPose ( + const Eigen::Vector3d& position, + const Eigen::Quaterniond& orientation) { + robot_position_ = position; + robot_orientation_ = orientation; + } + + /** + * @brief Computes the unwrapped yaw angle from the local copy of the robot's orientation. + * Stores the result in a member variable; + * @return Yaw angle + */ + double getRobotYaw (); + + /** + * @brief Sets a member variable of the full map and sets received_ flag to true. + * @param[in] map the full elevation map + * @param[in] o_T_pco the transformation from point_cloud_odom to odom frame + */ + virtual void setGridMap( + const grid_map::GridMap& map, + const Eigen::Isometry3d& o_T_pco); + + /** + * @brief Simplifies the acquired elevation map and stores the result in a member variable. + */ + void simplifyGridMap(); + + /** + * @brief Converts a layer of a gridmap to an image + * @param[in] layer_name the name of the layer from which to extract the gridmap + * @param[in] map the input gridmap + * @param[out] image the output image + */ + void convertGridMapToCvImage( + const std::string& layer_name, + const grid_map::GridMap& map, + cv::Mat& image, + const float lower_value = 0.0, + const float upper_value = 1.0); + + /** + * @brief Converts images of first- and second-order derivatives to layers of a gridmap + * @param[in] layer_name the common name of the layers + * @param[in] images the input images + * @param[out] map the output gridmap + */ + void convertCvImagesOfFirstAndSecondOrderDerivativesToGridMap( + const std::string& layer_name, + const MDD& images, + grid_map::GridMap& map); + + /** + * @brief Converts images of first-order derivatives to layers of a gridmap + * @param[in] layer_name the common name of the layers + * @param[in] images the input images + * @param[out] map the output gridmap + */ + void convertCvImagesOfFirstOrderDerivativesToGridMap( + const std::string& layer_name, + const D& images, + grid_map::GridMap& map); + + /** + * @brief Converts images of second-order derivatives to layers of a gridmap + * @param[in] layer_name the common name of the layers + * @param[in] images the input images + * @param[out] map the output gridmap + */ + void convertCvImagesOfSecondOrderDerivativesToGridMap( + const std::string& layer_name, + const DD& images, + grid_map::GridMap& map); + + /** + * @brief Converts an image to a layer of a gridmap + * @param[in] layer_name the name of the layer + * @param[in] image the input image + * @param[out] map the output gridmap + */ + void convertCvImageToGridMap( + const std::string& layer_name, + const cv::Mat& image, + grid_map::GridMap& map, + const float lower_value = 0.0, + const float upper_value = 1.0); + + /** + * @brief Filters an image using OpenCV filters by applying directional blur + * @param[in] image the input image + * @param[in/out] image_filtered the output image + */ + void applyDirectionalGaussianBlurToCvImage( + const cv::Mat& image, + cv::Mat& image_filtered); + + /** + * @brief Filters an image using OpenCV filters by applying first- and second-order (Sobel) derivatives + * @param[in] image the input image + * @param[in/out] images_filtered the output images (d, dx, dy, dd, dxdx, dxdy, dydx, dydy) + * @param[in] size the size of the kernel + * @param[in] denoise flag to apply Gaussian blur as a method of removing noise + */ + void applyFirstAndSecondOrderDerivativesToCvImage( + const cv::Mat& image, + MDD& images_filtered, + const int& size, + const bool& denoise); + + /** + * @brief Filters an image using OpenCV filters by applying second-order (Sobel) derivatives + * @param[in] image the input images (dx, dy) + * @param[in/out] images_filtered the output images (dd, dxdx, dxdy, dydx, dydy) + * @param[in] size the size of the kernel + * @param[in] denoise flag to apply Gaussian blur as a method of removing noise + */ + void applySecondOrderDerivativesToCvImage( + const D& image, + DD& images_filtered, + const int& size, + const bool& denoise); + + /** + * @brief Filters an image using OpenCV filters by applying first-order (Sobel) derivatives + * @param[in] image the input image + * @param[in/out] images_filtered the output images (d, dx, dy) + * @param[in] size the size of the kernel + * @param[in] denoise flag to apply Gaussian blur as a method of removing noise + */ + void applyFirstOrderDerivativesToCvImage( + const cv::Mat& image, + D& images_filtered, + const int& size, + const bool& denoise); + + /** + * @brief Filters an image using OpenCV filters by applying first-order (Sobel) derivative + * @param[in] image the input image + * @param[in/out] image_filtered the output image + * @param[in] size the size of the kernel + * @param[in] dim dimension {x, Y} with respect to which to take the derivative + * @param[in] denoise flag to apply Gaussian blur as a method of removing noise + */ + void applyDerivativeToCvImage( + const cv::Mat& image, + cv::Mat& image_filtered, + const int& size, + const Dim& dim, + const bool& denoise); + + /** + * @brief Filters an image using OpenCV filters by applying Gaussian blur + * @param[in] image the input image + * @param[in/out] image_filtered the output image + @param[in] size the size of the kernel + */ + void applyGaussianBlurToCvImage( + const cv::Mat& image, + cv::Mat& image_filtered, + const int& size); + + /** + * @brief Scales an image + * @param[in] scale the scaling factor + * @param[in] image the input image + * @param[in/out] image_scaled the output image + */ + void scaleCvImage( + const double& scale, + const cv::Mat& image, + cv::Mat& image_scaled); + + /** + * @brief Gets the Kernel dependent on robot's orientation (yaw) + * for the 2D filter (directional Gaussian blur) + */ + cv::Mat getDirectionalBlurKernel(); + + /** + * @brief Gets the simplified gridmap of the terrain + * @param[in/out] simplified_map the simplified map + * @param[in] scale an optional scaling factor + */ + void getSimplifiedGridMap( + grid_map::GridMap& simplified_map, + const double& scale = 1.0); + + /** + * @brief Applies a sequence of filters defined via the Filter Chain to a gridmap + * @param[in] map_in the input map without the filters applied + * @param[in/out] map_out the output map with the filters applied + */ + void applyFilterChain( + const grid_map::GridMap& map_in, + grid_map::GridMap& map_out); + + /** + * @brief Gets a value from a layer of the simplified map at a specific position + * @param layer name of the accessed layer + * @param position xy-coordinates + * @param is_inside flag that states whether the provided position is inside the map + * @return value at a position + */ + double getValueAtPosition( + const std::string& layer, + const Eigen::Vector2d& position, + bool& is_inside); + + /** + * @brief The run function for the TerrainSimplification + * which calls advance() in a while loop. + * It is started in a separate thread by a rosservice callback function. + */ + void run(); + + /** + * @brief The run function for the TerrainSimplification + * which calls advance() in a while loop. + * It is started in a separate thread by a rosservice callback function. + */ + void setRun(const bool& run) { run_ = run; } + + /** + * @brief Set the size of the simplified map + */ + void setMapSize(const Eigen::Vector2d& map_size) { map_size_ = map_size; } + + /** + * @brief Set the resolution of the simplified map + */ + void setMapResolution(double resolution) { grid_map_resolution_ = resolution; } + + /** + * @brief Set the size of the simplified map + */ + void setApplyFilterChain(bool apply_filter_chain) { apply_filter_chain_ = apply_filter_chain; } + + /** + * @brief Set the name of the input layer + */ + void setNameOfInputLayer(const std::string& layer) { + layer_ = layer; + } + + /** + * @brief Set the name of the input layer, if filter chain were applied + */ + void setNameOfInputLayerFiltered(const std::string& layer_filtered) { + layer_filtered_ = layer_filtered; + } + + /** + * @brief The advance function for the TerrainSimplification. + * It can be stopped by a rosservice call which sets the stop_ flag. + * @return true if successful + */ + bool advance(); + + /** + * @brief Returns the received_ flag indicating that the raw elevation map has been received + * @return true if received_ + */ + bool isReceived() { return received_; } + + /** + * @brief Returns the ready_ flag indicating that the simplified map has been created + * @return true if ready_ + */ + bool isReady() { return ready_; } + +private: + // Flags + bool run_ = true; ///< flag allowing run() to execute + bool received_ = false; ///< flag indicating that the raw elevation map has been received + bool ready_ = false; ///< flag indicating that the simplified map has been created + bool apply_filter_chain_ = true; ///< flag indicating that filter chain should be applied + double grid_map_resolution_ = 0.02; ///< grid map resolution + std::thread thread_run_; ///< thread for run() + std::atomic_bool thread_loop_; ///< flag for the thread + + // Transformations + Eigen::Isometry3d o_T_pco_; ///< a transformation from point_cloud_odom to odom frame + + // Layers + std::string layer_ = "elevation"; ///< name of the elevation layer in the + ///< incoming elevation map + std::string layer_filtered_ = "elevation"; ///< name of the filtered elevation layer + ///< in the incoming elevation map + + // Gridmaps + grid_map::GridMap map_full_pco_; ///< submap of the full elevation map in point_cloud_odom frame + grid_map::GridMap map_full_; ///< full elevation map + grid_map::GridMap map_sub_; ///< submap of the full elevation map + grid_map::GridMap map_sub_inpainted_; ///< inpainted submap of the full elevation map + grid_map::GridMap map_simplified_wo_traversability_; ///< filtered submap, without the Filter Chain applied (because the class is multi-threaded and applying the filter takes significant time) + grid_map::GridMap map_simplified_; ///< filtered submap, with the Filter Chain applied + grid_map::GridMap map_simplified_scaled_; ///< scaled filtered submap, with the Filter Chain applied + + // Gridmaps' parameters + Eigen::Vector2d map_size_; ///< size of the submap + int k_size_ = 51; ///< size of the kernel (for the directional Gaussian blur filter) + + // Filter Chain + std::shared_ptr > filter_chain_; ///< filter chain for traversability layer + + // Images + cv::Mat img_raw_; ///< image of the raw elevation map + cv::Mat img_inpainted_; ///< image of the inpainted elevation map + MD img_simplified_; ///< images of the simplified map and its first-order derivatives + MD img_simplified_scaled_; ///< scaled images of the simplified map and its first-order derivatives + MDD img_elevation_; ///< images of the elevation map and its second-order derivatives + MDD img_elevation_scaled_; ///< scaled images of the elevation map and its second-order derivatives + cv::Mat img_traversability_; ///< image of the traversability map + cv::Mat img_traversability_scaled_; ///< scaled image of the traversability map + cv::Mat img_slope_; ///< image of the slope map + cv::Mat img_slope_scaled_; ///< scaled image of the slope map + + // Robot's state + Eigen::Vector3d robot_position_; ///< robot's xyz position in the world (odom) frame + Eigen::Quaterniond robot_orientation_; ///< robot's base orientation (quaternion) in the base frame, aligned with the world frame + double yaw_prev_ = 0.0; ///< robot's base orientation (yaw) in the base frame, aligned with the world frame + + // Mutex + std::mutex mutex_; ///< mutex for map_ and img_ variables + std::mutex mutex_state_; ///< mutex for robot's position and orientation variables + +}; + +} //end namespace + +#endif //TERRAIN_SIMPLIFICATION_HPP diff --git a/terrain_simplification/package.xml b/terrain_simplification/package.xml new file mode 100644 index 0000000..0168ee4 --- /dev/null +++ b/terrain_simplification/package.xml @@ -0,0 +1,17 @@ + + + terrain_simplification + 1.0.0 + + A module that simplifies the observed terrain, represented as a gridmap, by applying a low-pass filter to extract the high-level geometry and qualify its irregularity. + + + Oliwier Melon + Oliwier Melon + BSD + + catkin + grid_map_core + grid_map_cv + grid_map_filters + diff --git a/terrain_simplification/src/terrain_simplification.cpp b/terrain_simplification/src/terrain_simplification.cpp new file mode 100644 index 0000000..e541c44 --- /dev/null +++ b/terrain_simplification/src/terrain_simplification.cpp @@ -0,0 +1,455 @@ +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace terrain_simplification { + +TerrainSimplification::TerrainSimplification() + : thread_loop_(true), + thread_run_(&TerrainSimplification::run, this) { +} + +TerrainSimplification::~TerrainSimplification() { + if (thread_run_.joinable()) { + thread_loop_ = false; + // Attempting to join the thread running the simplification with the main thread of TerrainSimplification... + thread_run_.join(); + } else { + std::cerr << "[TerrainSimplification::~TerrainSimplification] " + "The thread running the MPC could NOT be joined with " + "the main thread of TerrainSimplification: " + "thread_run_.joinable() = " << std::endl; + } +} + +void +TerrainSimplification::run() { + // continuously run the thread, until stopped + while (thread_loop_.load()) { + // IF flags true, advance the simplifying algorithm to produce to create simplified map + // ELSE wait for the flags + if (run_ && received_) { + advance(); + } else { + // wait 0.2 seconds + for (int i = 0; i < 100; i++) { + usleep(2000); // TODO: Use std::this_thread::sleep_for + } + } + } +} + +bool +TerrainSimplification::advance() { + simplifyGridMap(); + return true; +} + +void +TerrainSimplification::setGridMap( + const grid_map::GridMap& map, + const Eigen::Isometry3d& o_T_pco) { + mutex_.lock(); // to write map_full_pco_ + map_full_pco_ = map; + mutex_.unlock(); + o_T_pco_ = o_T_pco; + received_ = true; +} + +double +TerrainSimplification::getRobotYaw() { + std::lock_guard lock(mutex_state_); // to read and write + double yaw_prev = yaw_prev_; + const double q0 = robot_orientation_.w(); + const double q1 = robot_orientation_.x(); + const double q2 = robot_orientation_.y(); + const double q3 = robot_orientation_.z(); + double yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3)); + + // unwrap angle (see MATLAB's unwrap() for more info) + double d = yaw - yaw_prev; + d = d > M_PI ? d - 2.0*M_PI : (d < -M_PI ? d + 2.0*M_PI : d); + yaw_prev += d; + yaw_prev_ = yaw_prev; + return yaw_prev; +} + +void +TerrainSimplification::simplifyGridMap () { + bool success = false; + + // Extract the submap from the elevation map + mutex_state_.lock(); + grid_map::Position robot_position(robot_position_[0], robot_position_[1]); + mutex_state_.unlock(); + + mutex_.lock(); // to read map_full_pco_ + + if (apply_filter_chain_) { + map_full_ = map_full_pco_.getTransformedMap(o_T_pco_, layer_filtered_, "odom", 0.5); + } else { + map_full_ = map_full_pco_.getTransformedMap(o_T_pco_, layer_, "odom", 0.5); + } + + // check that robot position is inside the map + if (std::pow(std::pow(robot_position[0] - map_full_.getPosition()[0], 2.), 0.5) > map_full_.getLength()[0]/2. || + std::pow(std::pow(robot_position[1] - map_full_.getPosition()[1], 2.), 0.5) > map_full_.getLength()[1]/2.) { + std::cout << "[TerrainSimplification::simplifyGridMap] The position of the robot (" << robot_position[0] << ", " << robot_position[1] + << ") lays outside of the map centered at (" << map_full_.getPosition()[0] << ", " << map_full_.getPosition()[1] + << "), of size (" << map_full_.getLength()[0] << ", " << map_full_.getLength()[1] << "). Will reattempt." << std::endl; + mutex_.unlock(); + usleep(1000000); + return; + } + + map_sub_ = map_full_.getSubmap(robot_position, grid_map::Length(map_size_.x(), map_size_.y()), success); + mutex_.unlock(); + + // Create empty map + grid_map::GridMap map_simplified_wo_traversability; + map_simplified_wo_traversability.setFrameId("odom"); // TODO: hard-coded frame + map_simplified_wo_traversability.setGeometry(map_sub_.getLength(), grid_map_resolution_); // TODO: hard-coded parameter + map_simplified_wo_traversability.setPosition(robot_position); + + // Copy original layers + std::vector layers = { layer_, layer_filtered_ }; + if (!map_simplified_wo_traversability.addDataFrom(map_sub_, false, true, true, layers)) { // NOTE: use "false, true, false" to keep the same size, overwrite data, add just "layers" + ROS_ERROR("Could not add data from map_sub_ to map_simplified_wo_traversability."); + return; + } + + // Simplify the terrain + // convertGridMapToCvImagelayer_, map_sub_, img_raw_); // takes 0.5 ms + convertGridMapToCvImage(layer_filtered_, map_sub_, img_inpainted_); // takes 0.5 ms + MD img_simplified; + MDD img_elevation; + img_elevation.m = img_inpainted_; + applyDirectionalGaussianBlurToCvImage(img_elevation.m, img_simplified.m); // takes 1-10 ms + applyFirstOrderDerivativesToCvImage(img_simplified.m, img_simplified.f, 3, false); + // applyFirstAndSecondOrderDerivativesToCvImage(img_elevation.m, img_elevation, 3, true); + + // convertCvImagesOfFirstOrderDerivativesToGridMap("simplified", img_simplified.f, map_simplified_wo_traversability); + // convertCvImagesOfFirstAndSecondOrderDerivativesToGridMap(layer_filtered_, img_elevation, map_simplified_wo_traversability); + convertCvImageToGridMap("simplified", img_simplified.m, map_simplified_wo_traversability); // takes 1-2 ms + convertCvImageToGridMap(layer_filtered_, img_elevation.m, map_simplified_wo_traversability); // takes 1-2 ms + mutex_.lock(); // to write and img_simplified_ + img_simplified_ = img_simplified; + img_elevation_ = img_elevation; + mutex_.unlock(); + + // Apply the filter chain to create a separate map with a traversability layer + // grid_map::GridMap map_simplified({"simplified"}); + // if (apply_filter_chain_) { + // applyFilterChain(map_simplified_wo_traversability, map_simplified); // takes ~100-200 ms + + // mutex_.lock(); // to write map_simplified_wo_traversability_ and map_simplified_ + // map_simplified_wo_traversability_ = map_simplified_wo_traversability; + // map_simplified_ = map_simplified; + // ready_ = true; + // mutex_.unlock(); + // } else { + // mutex_.lock(); // to write and map_simplified_ + // map_simplified_ = map_simplified_wo_traversability; + // ready_ = true; + // mutex_.unlock(); + // } + mutex_.lock(); // to write and map_simplified_ + map_simplified_ = map_simplified_wo_traversability; + ready_ = true; + mutex_.unlock(); +} + +void +TerrainSimplification::convertGridMapToCvImage ( + const std::string& layer_name, + const grid_map::GridMap& map, + cv::Mat& image, + const float lower_value, + const float upper_value) { + grid_map::GridMapCvConverter::toImage( + map, layer_name, CV_16UC1, lower_value, upper_value, image); + +} + +void +TerrainSimplification::convertCvImagesOfFirstAndSecondOrderDerivativesToGridMap( + const std::string &layer_name, + const MDD &images, + grid_map::GridMap &map) { + convertCvImagesOfFirstOrderDerivativesToGridMap(layer_name, images.f, map); + convertCvImagesOfSecondOrderDerivativesToGridMap(layer_name, images.s, map); +} + +void +TerrainSimplification::convertCvImagesOfFirstOrderDerivativesToGridMap( + const std::string &layer_name, + const D &images, + grid_map::GridMap &map) { + convertCvImageToGridMap(layer_name + "_d", images.d, map); + convertCvImageToGridMap(layer_name + "_dx", images.dx, map); + convertCvImageToGridMap(layer_name + "_dy", images.dy, map); +} + +void +TerrainSimplification::convertCvImagesOfSecondOrderDerivativesToGridMap( + const std::string &layer_name, + const DD &images, + grid_map::GridMap &map) { +// convertCvImageToGridMap(layer_name + "_dd", images.dd, map); // note: currently .dd is not computed + convertCvImageToGridMap(layer_name + "_dxdx", images.dxdx, map); + convertCvImageToGridMap(layer_name + "_dxdy", images.dxdy, map); + convertCvImageToGridMap(layer_name + "_dydx", images.dydx, map); + convertCvImageToGridMap(layer_name + "_dydy", images.dydy, map); +} + +void +TerrainSimplification::convertCvImageToGridMap ( + const std::string& layer_name, + const cv::Mat& image, + grid_map::GridMap& map, + const float lower_value, + const float upper_value) { + if (!grid_map::GridMapCvConverter::addLayerFromImage( + image, layer_name, map, lower_value, upper_value)) { + mutex_.lock(); // to read map_size_ + std::cerr << "[TerrainSimplification::convertCvImageToGridMap] The size of the image (" << image.rows << ", " << image.cols + << ") does not correspond to grid map size (" << map.getSize()(0) << ", " << map.getSize()(1) + << "); make sure that these correspond to the specified map_size [m] (" << map_size_.x() << ", " << map_size_.y() + << ") and resolution (" << grid_map_resolution_ + << ")!" << std::endl; + mutex_.unlock(); + } +} + +void +TerrainSimplification::applyDirectionalGaussianBlurToCvImage ( + const cv::Mat& image, + cv::Mat& image_filtered) { + cv::Mat image_median; + cv::medianBlur(image, image_median, 5); + cv::Mat image_directional; + cv::filter2D(image_median, image_directional, -1, getDirectionalBlurKernel()); + + image_filtered = image_directional; +} + +void +TerrainSimplification::applyGaussianBlurToCvImage ( + const cv::Mat& image, + cv::Mat& image_filtered, + const int& size) { + cv::Mat image_gaussian; + cv::GaussianBlur(image, image_gaussian, cv::Size(size, size), 0.0, 0.0); + + image_filtered = image_gaussian; +} + +void +TerrainSimplification::applyDerivativeToCvImage( + const cv::Mat& image, + cv::Mat& image_filtered, + const int& size, + const Dim& dim, + const bool& denoise) { + // IF denoise, apply Gaussian Blur, ELSE copy image + cv::Mat image_denoised, image_denoised_signed; + if (denoise) { + cv::GaussianBlur(image, image_denoised, cv::Size(3,3), 0, 0); + } else { + image_denoised = image; + } + // Compute the gradient with respect to the specified dimension + cv::Mat image_grad, image_grad_unsigned; + // Convert from unsigned int [0, 65,535] to signed int [−32,767, +32,767] + image_denoised.convertTo(image_denoised_signed, CV_16S, 0.5); // note: multiplying by 0.5 + if (dim == X) { + cv::Sobel(image_denoised_signed, image_grad, CV_16S, 0, 1, size); // note: 0, 1 + } else if (dim == Y) { + cv::Sobel(image_denoised_signed, image_grad, CV_16S, 1, 0, size); // note: 1, 0 + } else { + image_grad = image_denoised_signed; + } + + image_grad.convertTo(image_grad_unsigned, CV_16UC1, 2.0, 32767.); // note: multiplying by 2.0 + image_filtered = image_grad_unsigned; +} + +void +TerrainSimplification::applyFirstOrderDerivativesToCvImage( + const cv::Mat& image, + D& images_filtered, + const int& size, + const bool& denoise) { + applyDerivativeToCvImage(image, images_filtered.dx, size, X, denoise); + applyDerivativeToCvImage(image, images_filtered.dy, size, Y, denoise); + + images_filtered.d = cv::Mat::zeros(images_filtered.dx.size().height, images_filtered.dx.size().width, CV_16UC1); + int n_rows = images_filtered.dx.size().height; + int n_cols = images_filtered.dx.size().width; + mutex_state_.lock(); + double yaw = yaw_prev_; + mutex_state_.unlock(); + double cos_yaw = cos(yaw); + double sin_yaw = sin(yaw); + for (int i = 0; i < n_rows; i++) { + unsigned short* d_row = images_filtered.d.ptr(i); + const unsigned short* dx_row = images_filtered.dx.ptr(i); + const unsigned short* dy_row = images_filtered.dy.ptr(i); + for (int j = 0; j < n_cols; j++) { + d_row[j] = static_cast(cos_yaw*(dx_row[j]-32767.) + sin_yaw*(dy_row[j]-32767.) + 32767.); + } + } +} + +void +TerrainSimplification::applySecondOrderDerivativesToCvImage( + const D& image, + DD& images_filtered, + const int& size, + const bool& denoise) { + applyDerivativeToCvImage(image.dx, images_filtered.dxdx, size, X, denoise); + applyDerivativeToCvImage(image.dx, images_filtered.dxdy, size, Y, denoise); + applyDerivativeToCvImage(image.dy, images_filtered.dydx, size, X, denoise); + applyDerivativeToCvImage(image.dy, images_filtered.dydy, size, Y, denoise); +} + +void +TerrainSimplification::applyFirstAndSecondOrderDerivativesToCvImage( + const cv::Mat& image, + MDD& images_filtered, + const int& size, + const bool& denoise) { + applyFirstOrderDerivativesToCvImage(image, images_filtered.f, size, denoise); + applySecondOrderDerivativesToCvImage(images_filtered.f, images_filtered.s, size, false); +} + +cv::Mat +TerrainSimplification::getDirectionalBlurKernel() { + cv::Mat k; + k = cv::Mat::zeros(k_size_, k_size_, CV_64F); + k.row((int)(k_size_-1)/2) = cv::getGaussianKernel(k_size_, -1, CV_64F).t(); + double yaw = getRobotYaw(); + cv::warpAffine( + k, + k, + cv::getRotationMatrix2D( + cv::Point2f((k_size_-1)/2,(k_size_-1)/2), + (double)yaw*180./M_PI-90, + 1.0), + cv::Size(k_size_, k_size_) + ); + k = k * (double)(1./ cv::sum(k)[0]); + return k; +} + +void +TerrainSimplification::applyFilterChain( + const grid_map::GridMap &map_in, + grid_map::GridMap &map_out) { + // Apply filter chain. + if (!filter_chain_->update(map_in, map_out)) { + ROS_ERROR("Could not update the grid map filter chain!"); + return; + } +} + +void +TerrainSimplification::getSimplifiedGridMap( + grid_map::GridMap& simplified_map, + const double& scale) { + + // IF the returned map is to be scaled ELSE not scaled + if (scale != 1.0) { + // Set up the map object + mutex_.lock(); // to read map_filtered_ + map_simplified_scaled_.setFrameId("odom"); // TODO: hard-coded parameter + // TODO: map_simplified_wo_traversability_ has not been set before... + map_simplified_scaled_.setGeometry(map_simplified_wo_traversability_.getLength(), grid_map_resolution_/scale); // TODO: hard-coded parameter + map_simplified_scaled_.setPosition(map_simplified_wo_traversability_.getPosition()); + mutex_.unlock(); + + // Convert GridMap layers to images + mutex_.lock(); // to read map_simplified_ + convertGridMapToCvImage("traversability", map_simplified_, img_traversability_); + convertGridMapToCvImage("slope", map_simplified_, img_slope_); + mutex_.unlock(); + + // Scale images + mutex_.lock(); // to read img_simplified_ + MD img_simplified = img_simplified_; + MDD img_elevation = img_elevation_; + mutex_.unlock(); + + scaleCvImage(scale, img_simplified.m, img_simplified_scaled_.m); + scaleCvImage(scale, img_simplified.f.d, img_simplified_scaled_.f.d); + scaleCvImage(scale, img_simplified.f.dx, img_simplified_scaled_.f.dx); + scaleCvImage(scale, img_simplified.f.dy, img_simplified_scaled_.f.dy); + + scaleCvImage(scale, img_elevation.m, img_elevation_scaled_.m); + scaleCvImage(scale, img_elevation.f.d, img_elevation_scaled_.f.d); + scaleCvImage(scale, img_elevation.f.dx, img_elevation_scaled_.f.dx); + scaleCvImage(scale, img_elevation.f.dy, img_elevation_scaled_.f.dy); + scaleCvImage(scale, img_elevation.s.dxdx, img_elevation_scaled_.s.dxdx); + scaleCvImage(scale, img_elevation.s.dxdy, img_elevation_scaled_.s.dxdy); + scaleCvImage(scale, img_elevation.s.dydx, img_elevation_scaled_.s.dydx); + scaleCvImage(scale, img_elevation.s.dydy, img_elevation_scaled_.s.dydy); + + scaleCvImage(scale, img_traversability_, img_traversability_scaled_); + scaleCvImage(scale, img_slope_, img_slope_scaled_); + + // Convert scaled images to GridMap layers + convertCvImageToGridMap("slope", img_slope_scaled_, map_simplified_scaled_); + convertCvImageToGridMap("traversability", img_traversability_scaled_, map_simplified_scaled_); + convertCvImageToGridMap("simplified", img_simplified_scaled_.m, map_simplified_scaled_); + convertCvImagesOfFirstOrderDerivativesToGridMap("simplified", img_simplified_scaled_.f, map_simplified_scaled_); + convertCvImagesOfFirstAndSecondOrderDerivativesToGridMap(layer_, img_elevation_scaled_, map_simplified_scaled_); + convertCvImageToGridMap(layer_, img_elevation_scaled_.m, map_simplified_scaled_); // takes 1-2 ms + + simplified_map = map_simplified_scaled_; + } else { + mutex_.lock(); // to read map_simplified_ + simplified_map = map_simplified_; + mutex_.unlock(); + } +} + +void +TerrainSimplification::scaleCvImage( + const double &scale, + const cv::Mat &image, + cv::Mat &image_scaled) { + int size_x = (int) (scale*image.size().width); + int size_y = (int) (scale*image.size().height); + image_scaled = cv::Mat::zeros(size_x, size_y, CV_16UC1); + cv::resize(image, image_scaled, cv::Size(size_x, size_y)); +} + +double +TerrainSimplification::getValueAtPosition( + const std::string& layer, + const Eigen::Vector2d& position, + bool& is_inside) { + grid_map::Position p(position); + std::lock_guard lock(mutex_); + double value = 0.0; + if (map_simplified_.isInside(p)) { + is_inside = true; + value = map_simplified_.atPosition(layer, p); + } else { + std::cerr << "[TerrainSimplification::getValueAtPosition] Error: the queried position (" << position + << ") lays outside of the map centered at (" << map_simplified_.getPosition()[0] << ", " << map_simplified_.getPosition()[1] + << "), of size (" << map_simplified_.getLength()[0] << ", " << map_simplified_.getLength()[1] << ")." << std::endl; + is_inside = false; + } + return value; +} +} diff --git a/terrain_simplification/terrain-simplification_base-motion-constraint_5x-speed.gif b/terrain_simplification/terrain-simplification_base-motion-constraint_5x-speed.gif new file mode 100644 index 0000000..fb342b4 Binary files /dev/null and b/terrain_simplification/terrain-simplification_base-motion-constraint_5x-speed.gif differ diff --git a/terrain_simplification/terrain-simplification_base-motion-constraint_simulation.gif b/terrain_simplification/terrain-simplification_base-motion-constraint_simulation.gif new file mode 100644 index 0000000..57c293c Binary files /dev/null and b/terrain_simplification/terrain-simplification_base-motion-constraint_simulation.gif differ diff --git a/terrain_simplification/terrain-simplification_derivatives.png b/terrain_simplification/terrain-simplification_derivatives.png new file mode 100644 index 0000000..151c877 Binary files /dev/null and b/terrain_simplification/terrain-simplification_derivatives.png differ diff --git a/terrain_simplification/terrain-simplification_traversability.png b/terrain_simplification/terrain-simplification_traversability.png new file mode 100644 index 0000000..be3ea2a Binary files /dev/null and b/terrain_simplification/terrain-simplification_traversability.png differ diff --git a/terrain_simplification_ros/CMakeLists.txt b/terrain_simplification_ros/CMakeLists.txt new file mode 100644 index 0000000..1f07532 --- /dev/null +++ b/terrain_simplification_ros/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.0.2) +project(terrain_simplification_ros) + +set(CMAKE_CXX_STANDARD 14) +set(DEFAULT_BUILD "Release") +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + message(STATUS "Setting build type to '${DEFAULT_BUILD}' as none was specified.") + set(CMAKE_BUILD_TYPE ${DEFAULT_BUILD} CACHE STRING "Choose the type of build." FORCE) + # Set the possible values of build type for cmake-gui + set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" + "Release" + "MinSizeRel" + "RelWithDebInfo") +endif() + +add_definitions(-DMELO_FUNCTION_PRINTS) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rosbag + std_msgs + geometry_msgs + visualization_msgs + grid_map_core + grid_map_filters + grid_map_cv + message_generation + terrain_simplification + tf_conversions + ) + +add_service_files( + FILES + GetValueAtPosition.srv +) + +generate_messages() + + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS roscpp + grid_map_core + grid_map_filters + grid_map_cv + message_runtime + std_msgs + geometry_msgs + terrain_simplification + ) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ) + +add_library(${PROJECT_NAME} + src/terrain_simplification_ros.cpp + ) + +add_dependencies(${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + +add_executable(${PROJECT_NAME}_node + src/terrain_simplification_ros_node.cpp + ) + +add_dependencies(${PROJECT_NAME}_node + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + +add_executable(${PROJECT_NAME}_service_caller + src/terrain_simplification_ros_service_caller.cpp + ) + +target_link_libraries(${PROJECT_NAME}_service_caller + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + +############# +## Install ## +############# +# Mark library for installation +install( + TARGETS + ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +# Mark header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) + +# Mark other files for installation +install( + DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/terrain_simplification_ros/README.md b/terrain_simplification_ros/README.md new file mode 100644 index 0000000..8ffcd6a --- /dev/null +++ b/terrain_simplification_ros/README.md @@ -0,0 +1,30 @@ +# Description +See https://wiki.oxfordrobots.com/display/~oliwier/2021/01/07/Terrain+Simplification + +# How to Run +1. To launch the main ros node which computes the simplified terrain: +``` +roslaunch terrain_simplification_ros terrain_simplification_ros.launch +``` +2. To launch an auxiliry node which calls the gridmap publisher of the main node, a specified rate (default = 10 Hz): +``` +roslaunch terrain_simplification_ros terrain_simplification_ros_service_caller.launch +``` + +# Parameters +To set the rate of the publisher: +``` +rosparam set /terrain_simplification_ros_service_caller/rate 1 +``` + +Some (most useful) ROS parameters (defaults are set in `config/config.yaml`): +``` +/terrain_simplification_ros_node/map_res_scaling +/terrain_simplification_ros_node/gridmap_size_x +/terrain_simplification_ros_node/gridmap_size_y +/terrain_simplification_ros_node/h_nominal +``` +These can be set and re-read using: +``` +rosservice call /terrain_simplification/read +``` diff --git a/terrain_simplification_ros/config/config.yaml b/terrain_simplification_ros/config/config.yaml new file mode 100644 index 0000000..476fecf --- /dev/null +++ b/terrain_simplification_ros/config/config.yaml @@ -0,0 +1,16 @@ +terrain_simplification/topic_elevation_map: /elevation_mapping/elevation_map +terrain_simplification/topic_robot_state: /state_estimator/pose_in_odom +terrain_simplification/topic_map_simplified: /terrain_simplification/map_simplified + +terrain_simplification/gridmap_size_x: 2.5 +terrain_simplification/gridmap_size_y: 2.5 +terrain_simplification/layer: "elevation" +terrain_simplification/layer_filtered: "elevation_inpainted" +terrain_simplification/h_nominal: 0.53 +terrain_simplification/map_res_scaling: 1.0 +terrain_simplification/map_resolution: 0.02 #0.04 # 0.02 + +terrain_simplification/erode_radius: 0.11 +terrain_simplification/traversability_threshold: 0.7 +terrain_simplification/verbose_timer: true +terrain_simplification/filter_chain_parameter_name: grid_map_filters diff --git a/terrain_simplification_ros/config/filter_chain.yaml b/terrain_simplification_ros/config/filter_chain.yaml new file mode 100644 index 0000000..2deb6a9 --- /dev/null +++ b/terrain_simplification_ros/config/filter_chain.yaml @@ -0,0 +1,92 @@ +grid_map_filters: + + - name: buffer_normalizer + type: gridMapFilters/BufferNormalizerFilter + + - name: inpaint_radially + type: gridMapFiltersDrs/RadialInpaintFilter + params: + input_layer: elevation + output_layer: elevation_inpainted + radius: 0.05 # in m + pre_denoising: false # enables denoising before inpainting + denoising_radius: 0.15 # m + + # # Fill holes in the map with inpainting. + # - name: inpaint + # type: gridMapCv/InpaintFilter + # params: + # input_layer: elevation + # output_layer: elevation_inpainted + # radius: 0.05 + + # # Compute surface normals. + # - name: surface_normals + # type: gridMapFilters/NormalVectorsFilter + # params: + # parallelization_enabled: true + # thread_number: 4 + # algorithm: area + # input_layer: elevation_inpainted + # output_layers_prefix: normal_vectors_ + # radius: 0.03 + # normal_vector_positive_axis: z + +# # Delete layers. +# - name: delete_original_layers +# type: gridMapFilters/DeletionFilter +# params: +# layers: [color, upper_bound, lower_bound, uncertainty_range, normal_vectors_x, normal_vectors_y,] # List of layers. + + + # # Compute slope from surface normal. + # - name: slope + # type: gridMapFilters/MathExpressionFilter + # params: + # output_layer: slope + # expression: acos(normal_vectors_z) + + # # Reduce noise with a radial blurring filter. + # - name: mean_in_radius2 + # type: gridMapFilters/MeanInRadiusFilter + # params: + # input_layer: slope + # output_layer: slope_smooth + # radius: 0.4 + + # # Boxblur as an alternative to the inpaint and radial blurring filter above. + # - name: boxblur + # type: gridMapFilters/SlidingWindowMathExpressionFilter + # params: + # input_layer: slope + # output_layer: slope_smooth + # expression: meanOfFinites(slope) + # compute_empty_cells: true + # edge_handling: crop # options: inside, crop, empty, mean + # window_size: 21 # optional + + + # # Compute traversability as normalized weighted sum of slope and roughness. + # - name: traversability + # type: gridMapFilters/MathExpressionFilter + # params: + # output_layer: traversability + # expression: (1.0 - (slope_smooth / 0.6)) + + # # Set lower threshold on traversability. + # - name: traversability_lower_threshold + # type: gridMapFilters/ThresholdFilter + # params: + # condition_layer: traversability + # output_layer: traversability + # lower_threshold: 0.0 + # set_to: 0.0 + + # # Set upper threshold on traversability. + # - name: traversability_upper_threshold + # type: gridMapFilters/ThresholdFilter + # params: + # condition_layer: traversability + # output_layer: traversability + # upper_threshold: 1.0 + # set_to: 1.0 # Other uses: .nan, .inf diff --git a/terrain_simplification_ros/include/terrain_simplification_ros/terrain_simplification_ros.hpp b/terrain_simplification_ros/include/terrain_simplification_ros/terrain_simplification_ros.hpp new file mode 100644 index 0000000..cf9dc89 --- /dev/null +++ b/terrain_simplification_ros/include/terrain_simplification_ros/terrain_simplification_ros.hpp @@ -0,0 +1,194 @@ +/** + * @file terrain_simplification_ros.hpp + * @brief A ros interface to the TerrainSimplification class, + * @author Oliwier Melon (omelon@robots.ox.ac.uk) + * @bug No known bugs. + * @date 07/01/2021 + * @version 1.1 + * @copyright 2020, Oliwier Melon. BSD-3-Clause + */ + +#ifndef TERRAIN_SIMPLIFICATION_ROS_HPP +#define TERRAIN_SIMPLIFICATION_ROS_HPP + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace terrain_simplification_ros { + +class TerrainSimplificationRos { + +public: + TerrainSimplificationRos(); + + /** + * @brief Construct the ROS-based interface to the terrain simplification module. + * @param node_handle + */ + TerrainSimplificationRos(ros::NodeHandle& nh); + ~TerrainSimplificationRos() = default; + + /** + * @brief Get the pointer to the base-class object + * @returns shared_ptr of the base-class object + */ + std::shared_ptr getTerrainSimplification() { return terr_simp_; } + +protected: + /** + * @brief The callback function used to get the elevation map + * @param[in] msg elevation map message + */ + void subGridMap( + const grid_map_msgs::GridMap& msg); + + /** + * @brief Directly sets the gridmap member variable of the the base class. + * @param[in] msg elevation map message + */ + void setGridMap( + const grid_map_msgs::GridMap& msg); + + /** + * @brief The callback function used to get the pose of the robot + * @param[in] msg pose message + */ + void subRobotPose( + const geometry_msgs::PoseWithCovarianceStamped& msg); + + /** + * @brief Directly sets the position and orientation member variables of the the base class. + * @param[in] msg pose message + */ + void setRobotPose( + const geometry_msgs::PoseWithCovarianceStamped& msg); + + /** + * @brief Publish the simplified map + */ + void pubSimplifiedGridMap(); + + /** + * @brief Read ROS parameters + */ + bool readParameters(); + + /** + * @brief The callback function used to start the run function in TerrainSimplification + */ + bool run( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response); + + /** + * @brief The callback function used to stop the run function in TerrainSimplification + */ + bool stop( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response); + + /** + * @brief The callback function used to publish the simplified map + */ + bool pub( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response); + + /** + * @brief The callback function used to read ros parameters + */ + bool read( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response); + + /** + * @brief Gets a value from a layer of the simplified map at a specific position + * @param layer name of the accessed layer + * @param position xy-coordinates + * @return value at a position + */ + bool getValueAtPosition( + terrain_simplification_ros::GetValueAtPosition::Request& request, + terrain_simplification_ros::GetValueAtPosition::Response& response); + + /** + * @brief Obtains the height of the simplified map with the nominal height offset (h_nominal) applied by default + * @param location xy-coordinates of a location + * @param is_inside flag that states whether the provided position is inside the map + * @param apply_h_offset flag to apply the nominal height offset + */ + double getHeight( + const Eigen::Vector2d& location, + bool& is_inside, + const bool& apply_h_offset = true); + + /** + * @brief Obtains the traversability of the traversability + * @param location xy-coordinates of a location + * @param is_inside flag that states whether the provided position is inside the map + */ + double getTraversability( + const Eigen::Vector2d& location, + bool& is_inside); + +private: + std::shared_ptr terr_simp_; ///< pointer to the base-class object + + // TF + std::shared_ptr tf_listener_; + + // ROS + ros::NodeHandle ros_nh_; ///< node handle + ros::Subscriber ros_sub_robot_pose_; ///< subscriber robot pose + ros::Subscriber ros_sub_map_; ///< subscriber elevation map + ros::Publisher ros_pub_map_; ///< publisher simplified map + ros::ServiceServer ros_server_run_; ///< service server to start terrain simplification + ros::ServiceServer ros_server_stop_; ///< service server to stop terrain simplification + ros::ServiceServer ros_server_pub_; ///< service server to publish simplified map + ros::ServiceServer ros_server_read_; ///< service server to read ros parameters + ros::ServiceServer ros_server_get_val_; ///< service server to get value at position + std::string topic_elevation_map_; ///< topic name of the elevation map + std::string topic_robot_state_; ///< topic name of the robot state + std::string topic_map_simplified_; ///< topic name of the simplified map + + bool is_run_ = true; ///< flag indicating that the terrain simplification has been commanded to run + + std::shared_ptr > filter_chain_; ///< filter chain for traversability layer + std::string filter_chain_parameters_name_; ///< parameters for the filter chain + + // Robot pose + geometry_msgs::PoseWithCovarianceStamped msg_robot_pose_; ///< message robot pose + + // Parameters + Eigen::Vector2d map_size_; ///< size of simplified map + double h_nominal_ = 0.53; ///< nominal robot height + double map_res_scaling_ = 0.4; ///< map resolution scaling + double map_resolution_ = 0.02; ///< map resolution + + // GridMap + grid_map::GridMap map_sub_; ///< submap (of size map_size_) of the elevation map + grid_map::GridMap map_simplified_; ///< the simplified map + + // Layers + std::string layer_; ///< name of the elevation layer in the incoming elevation map + std::string layer_filtered_; ///< name of the filtered elevation layer + ///< in the incoming elevation map + +}; +} + +#endif //TERRAIN_SIMPLIFICATION_ROS_HPP diff --git a/terrain_simplification_ros/launch/terrain_simplification_ros.launch b/terrain_simplification_ros/launch/terrain_simplification_ros.launch new file mode 100644 index 0000000..b8f1196 --- /dev/null +++ b/terrain_simplification_ros/launch/terrain_simplification_ros.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + diff --git a/terrain_simplification_ros/launch/terrain_simplification_ros_service_caller.launch b/terrain_simplification_ros/launch/terrain_simplification_ros_service_caller.launch new file mode 100644 index 0000000..527499d --- /dev/null +++ b/terrain_simplification_ros/launch/terrain_simplification_ros_service_caller.launch @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + diff --git a/terrain_simplification_ros/package.xml b/terrain_simplification_ros/package.xml new file mode 100644 index 0000000..60d51c1 --- /dev/null +++ b/terrain_simplification_ros/package.xml @@ -0,0 +1,28 @@ + + + terrain_simplification_ros + 1.0.0 + + A ROS-based interface to terrain_simplification package. + + + Oliwier Melon + Oliwier Melon + BSD + + catkin + + message_runtime + message_generation + std_msgs + geometry_msgs + roscpp + rosbag + grid_map_core + grid_map_cv + grid_map_filters + terrain_simplification + rviz + rqt_bag + grid_map_filters_drs + diff --git a/terrain_simplification_ros/src/terrain_simplification_ros.cpp b/terrain_simplification_ros/src/terrain_simplification_ros.cpp new file mode 100644 index 0000000..f03d0b1 --- /dev/null +++ b/terrain_simplification_ros/src/terrain_simplification_ros.cpp @@ -0,0 +1,214 @@ +#include "terrain_simplification_ros/terrain_simplification_ros.hpp" + +namespace terrain_simplification_ros { + +TerrainSimplificationRos::TerrainSimplificationRos(ros::NodeHandle& nh) + : ros_nh_(nh) { + terr_simp_ = std::make_shared(); + + // Read ros parameters + if (!readParameters()) { + throw std::runtime_error("Could not read parameters"); + } + + // Setup filter chain + filter_chain_ = std::make_shared >("grid_map::GridMap"); + if (!filter_chain_->configure(filter_chain_parameters_name_, ros_nh_)){ + throw std::runtime_error("Could not configure the filter chain!"); + } + // TF + tf_listener_ = std::make_shared(); // TODO: Upgrade to tf2_ros::TransformListener + + terr_simp_->setFilterChain(filter_chain_); + terr_simp_->setNameOfInputLayer(layer_); + terr_simp_->setNameOfInputLayerFiltered(layer_filtered_); + + // ROS + ros_sub_robot_pose_ = ros_nh_.subscribe(topic_robot_state_, 2, &TerrainSimplificationRos::subRobotPose, this); + ros_sub_map_ = ros_nh_.subscribe(topic_elevation_map_, 2, &TerrainSimplificationRos::subGridMap, this); + ros_pub_map_ = ros_nh_.advertise(topic_map_simplified_, 1, false); + // TODO: Do not fix the global name. These should be advertised locally to the node handle! + ros_server_run_ = ros_nh_.advertiseService("/terrain_simplification/run", &TerrainSimplificationRos::run, this); + ros_server_stop_ = ros_nh_.advertiseService("/terrain_simplification/stop", &TerrainSimplificationRos::stop, this); + ros_server_pub_ = ros_nh_.advertiseService("/terrain_simplification/pub", &TerrainSimplificationRos::pub, this); + ros_server_read_ = ros_nh_.advertiseService("/terrain_simplification/read", &TerrainSimplificationRos::read, this); + ros_server_get_val_ = ros_nh_.advertiseService("/terrain_simplification/get_val", &TerrainSimplificationRos::getValueAtPosition, this); +} + +bool TerrainSimplificationRos::run( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response) { + terr_simp_->setRun(true); + is_run_ = true; + return is_run_; +} + +bool TerrainSimplificationRos::stop( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response) { + terr_simp_->setRun(false); + return true; +} + +bool TerrainSimplificationRos::pub( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response) { + if (!is_run_) { + ROS_WARN_STREAM("TerrainSimplification is not running. " + "It must be started via the /run service call. "); + } else if (terr_simp_->isReady()) { + pubSimplifiedGridMap(); + } else { + if (!terr_simp_->isReceived()) { + ROS_WARN_STREAM("The elevation map was not received. " + "It either must be published on topic = " << topic_elevation_map_ << + " or manually provided via setGridMap()."); + } else { + ROS_WARN_STREAM("The simplification process has not finished; the gridmap is not ready."); + } + } + return true; +} + +bool TerrainSimplificationRos::read( + std_srvs::Empty::Request& request, + std_srvs::Empty::Response& response) { + bool success = readParameters(); + return success; +} + +bool TerrainSimplificationRos::getValueAtPosition( + terrain_simplification_ros::GetValueAtPosition::Request &request, + terrain_simplification_ros::GetValueAtPosition::Response &response) { + Eigen::Vector2d p(request.x, request.y); + bool is_inside = false; + double val = terr_simp_->getValueAtPosition(request.layer, p, is_inside); + + if (is_inside) { + if (request.layer == "simplified") { + response.val = val + h_nominal_; + } else { + response.val = val; + } + } else { + ROS_ERROR_STREAM("The provided position (" << p.transpose() << ") " + "is outside the map; " + "hence, the corresponding value cannot be obtained."); + } + return is_inside; +} + +double TerrainSimplificationRos::getHeight( + const Eigen::Vector2d& location, + bool& is_inside, + const bool& apply_h_offset) { + double h = terr_simp_->getValueAtPosition("simplified", location, is_inside); + if (apply_h_offset) h += h_nominal_; + return h; +} + +double TerrainSimplificationRos::getTraversability( + const Eigen::Vector2d& location, + bool& is_inside) { + return terr_simp_->getValueAtPosition("traversability", location, is_inside); +} + +bool +TerrainSimplificationRos::readParameters() { + // TODO: These parameters should be local to the namespace of the node. + if (!ros_nh_.getParam("/terrain_simplification/topic_elevation_map", topic_elevation_map_)){ + ROS_ERROR("Could not read parameter `terrain_simplification/topic_elevation_map`."); + return false; + } + if (!ros_nh_.getParam("/terrain_simplification/topic_robot_state", topic_robot_state_)){ + ROS_ERROR("Could not read parameter `terrain_simplification/topic_robot_state`."); + return false; + } + if (!ros_nh_.getParam("/terrain_simplification/topic_map_simplified", topic_map_simplified_)){ + ROS_ERROR("Could not read parameter `terrain_simplification/topic_robot_state`."); + return false; + } + if (!ros_nh_.getParam("/terrain_simplification/filter_chain_parameter_name", filter_chain_parameters_name_)){ + ROS_ERROR("Could not read parameter `terrain_simplification/filter_chain_parameter_name`."); + return false; + } + + ros_nh_.param("/terrain_simplification/layer", layer_, "elevation"); + ros_nh_.param("/terrain_simplification/layer_filtered", layer_filtered_, "elevation_inpainted"); + ros_nh_.param("/terrain_simplification/gridmap_size_x", map_size_.x(), 2.5); + ros_nh_.param("/terrain_simplification/gridmap_size_y", map_size_.y(), 2.5); + ros_nh_.param("/terrain_simplification/h_nominal", h_nominal_, 0.53); + ros_nh_.param("/terrain_simplification/map_res_scaling", map_res_scaling_, 0.4); + ros_nh_.param("/terrain_simplification/map_resolution", map_resolution_, 0.02); + + terr_simp_->setMapSize(map_size_); + terr_simp_->setMapResolution(map_resolution_); + + return true; +} + +void +TerrainSimplificationRos::subRobotPose( + const geometry_msgs::PoseWithCovarianceStamped& msg) { + msg_robot_pose_ = msg; + setRobotPose(msg); +} + +void +TerrainSimplificationRos::setRobotPose( + const geometry_msgs::PoseWithCovarianceStamped &msg) { + Eigen::Vector3d p ( + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z); + Eigen::Quaterniond o ( + msg.pose.pose.orientation.w, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z); + terr_simp_->setRobotPose(p, o); +} + +void +TerrainSimplificationRos::subGridMap( + const grid_map_msgs::GridMap& msg) { + setGridMap(msg); +} + +void +TerrainSimplificationRos::setGridMap( + const grid_map_msgs::GridMap& msg) { + grid_map::GridMap map; + grid_map::GridMapRosConverter::fromMessage(msg, map); + + + // Get transformation to convert point clouds into base frame + tf::StampedTransform o_T_pco_transform; + Eigen::Isometry3d o_T_pco; + tf_listener_->waitForTransform("odom", "point_cloud_odom", ros::Time(0), ros::Duration(2.0)); // TODO: hard-coded frame names + try { + tf_listener_->lookupTransform("odom", "point_cloud_odom", ros::Time(0), o_T_pco_transform); // TODO: hard-coded frame names + tf::transformTFToEigen (o_T_pco_transform, o_T_pco); + } catch (const tf::TransformException& e) { + ROS_ERROR_STREAM("Couldn't find transform from frame [" << "point_cloud_odom" << "] to base frame [" << "odom" << "]. " << e.what()); + } + + grid_map::GridMap map_filtered; + filter_chain_->update(map, map_filtered); + + terr_simp_->setGridMap(map_filtered, o_T_pco); +} + +void +TerrainSimplificationRos::pubSimplifiedGridMap() { + grid_map::GridMap map, map_h_offset; + terr_simp_->getSimplifiedGridMap(map, map_res_scaling_); + Eigen::Isometry3d transf = Eigen::Isometry3d::Identity(); + transf.translation().z() = h_nominal_; + map_h_offset = map.getTransformedMap(transf, "simplified", "odom", 0.0); // TODO: hard-coded frame name + + grid_map_msgs::GridMap msg; + grid_map::GridMapRosConverter::toMessage(map_h_offset, msg); + ros_pub_map_.publish(msg); +} +} diff --git a/terrain_simplification_ros/src/terrain_simplification_ros_node.cpp b/terrain_simplification_ros/src/terrain_simplification_ros_node.cpp new file mode 100644 index 0000000..14301c1 --- /dev/null +++ b/terrain_simplification_ros/src/terrain_simplification_ros_node.cpp @@ -0,0 +1,13 @@ +#include "terrain_simplification_ros/terrain_simplification_ros.hpp" + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "terrain_simplification"); + ros::NodeHandle node_handle("~"); + terrain_simplification_ros::TerrainSimplificationRos terrain_simplification_ros(node_handle); + ros::AsyncSpinner spinner(2); + spinner.start(); + ros::waitForShutdown(); + + return 0; +} diff --git a/terrain_simplification_ros/src/terrain_simplification_ros_service_caller.cpp b/terrain_simplification_ros/src/terrain_simplification_ros_service_caller.cpp new file mode 100644 index 0000000..8047907 --- /dev/null +++ b/terrain_simplification_ros/src/terrain_simplification_ros_service_caller.cpp @@ -0,0 +1,23 @@ +#include "ros/ros.h" +#include +#include + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "terrain_simplification_service_caller"); + ros::NodeHandle nh; + ros::ServiceClient ros_client_pub + = nh.serviceClient("/terrain_simplification/pub"); + std_srvs::Empty::Request req; + std_srvs::Empty::Response res; + + double rate = 10.; + while (ros::ok()) + { + nh.param("/terrain_simplification_ros_service_caller/rate", rate, 10.); + ros_client_pub.call(req,res); + ros::Rate r(rate); + r.sleep(); + } + return 0; +} diff --git a/terrain_simplification_ros/srv/GetValueAtPosition.srv b/terrain_simplification_ros/srv/GetValueAtPosition.srv new file mode 100644 index 0000000..c763171 --- /dev/null +++ b/terrain_simplification_ros/srv/GetValueAtPosition.srv @@ -0,0 +1,5 @@ +string layer +float64 x +float64 y +--- +float64 val