From 66ad5c3e36f2f04767c93abad4741c9f81d1d9ce Mon Sep 17 00:00:00 2001 From: amilearning Date: Fri, 1 Nov 2024 13:08:22 +0000 Subject: [PATCH 01/14] wrapper ros2 done --- elevation_map_msgs/CMakeLists.txt | 65 +- elevation_map_msgs/msg/ChannelInfo.msg | 2 +- elevation_map_msgs/msg/Statistics.msg | 4 +- elevation_map_msgs/package.xml | 21 +- elevation_mapping_cupy/CMakeLists.txt | 112 +- .../elevation_mapping_ros.hpp | 313 ++-- .../elevation_mapping_wrapper.hpp | 21 +- elevation_mapping_cupy/package.xml | 22 +- .../src/elevation_mapping_node.cpp | 20 +- .../src/elevation_mapping_ros.cpp | 1638 ++++++++--------- .../src/elevation_mapping_wrapper.cpp | 209 ++- 11 files changed, 1249 insertions(+), 1178 deletions(-) diff --git a/elevation_map_msgs/CMakeLists.txt b/elevation_map_msgs/CMakeLists.txt index 2d73f5e5..0ea3f7cb 100644 --- a/elevation_map_msgs/CMakeLists.txt +++ b/elevation_map_msgs/CMakeLists.txt @@ -1,32 +1,55 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(elevation_map_msgs) -find_package(catkin REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) # Added std_msgs -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - message_generation +set(msg_files + "msg/Statistics.msg" + "msg/ChannelInfo.msg" ) -add_message_files( - FILES - Statistics.msg - ChannelInfo.msg +set(srv_files + "srv/CheckSafety.srv" + "srv/Initialize.srv" ) -## Generate services in the 'srv' folder -add_service_files( - FILES - CheckSafety.srv - Initialize.srv +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES geometry_msgs std_msgs # Added std_msgs ) +ament_export_dependencies(rosidl_default_runtime) -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - geometry_msgs -) +ament_package() + +# cmake_minimum_required(VERSION 3.8) +# project(elevation_map_msgs) + +# if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") +# add_compile_options(-Wall -Wextra -Wpedantic) +# endif() + +# # find dependencies +# find_package(ament_cmake REQUIRED) +# find_package(rosidl_default_generators REQUIRED) +# find_package(builtin_interfaces REQUIRED) +# find_package(std_msgs REQUIRED) +# find_package(geometry_msgs REQUIRED) +# find_package(action_msgs REQUIRED) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/Statistics.msg" +# "msg/ChannelInfo.msg" +# "srv/CheckSafety.srv" +# "srv/Initialize.srv" +# DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs +# ) + +# ament_export_dependencies(rosidl_default_runtime) + +# ament_package() -catkin_package( -) diff --git a/elevation_map_msgs/msg/ChannelInfo.msg b/elevation_map_msgs/msg/ChannelInfo.msg index bccf1447..2e6327ec 100644 --- a/elevation_map_msgs/msg/ChannelInfo.msg +++ b/elevation_map_msgs/msg/ChannelInfo.msg @@ -1,2 +1,2 @@ -Header header +std_msgs/Header header string[] channels # channel names for each layer \ No newline at end of file diff --git a/elevation_map_msgs/msg/Statistics.msg b/elevation_map_msgs/msg/Statistics.msg index 294ccb9d..85daaa50 100644 --- a/elevation_map_msgs/msg/Statistics.msg +++ b/elevation_map_msgs/msg/Statistics.msg @@ -1,2 +1,2 @@ - Header header - float64 pointcloud_process_fps +std_msgs/Header header +float64 pointcloud_process_fps diff --git a/elevation_map_msgs/package.xml b/elevation_map_msgs/package.xml index 5fa7a60a..22f222c1 100644 --- a/elevation_map_msgs/package.xml +++ b/elevation_map_msgs/package.xml @@ -1,5 +1,5 @@ - + elevation_map_msgs 0.0.0 ROS Message definitions for elevation mapping. @@ -7,13 +7,24 @@ Takahiro Miki MIT - catkin - + + ament_cmake + geometry_msgs - message_generation + rosidl_default_generators + std_msgs + rosidl_interface_packages + + geometry_msgs + rosidl_default_runtime + std_msgs - geometry_msgs + + ament_lint_auto + ament_lint_common + ament_cmake + rosidl_interface_packages diff --git a/elevation_mapping_cupy/CMakeLists.txt b/elevation_mapping_cupy/CMakeLists.txt index bc4cf6a0..81baa06d 100644 --- a/elevation_mapping_cupy/CMakeLists.txt +++ b/elevation_mapping_cupy/CMakeLists.txt @@ -1,80 +1,100 @@ -# %Tag(FULLTEXT)% -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(elevation_mapping_cupy) +# Enable C++11 (or higher if needed for ROS 2 and pybind11) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# Compiler options +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() +# Additional dependencies +find_package(Python COMPONENTS Interpreter Development) find_package(PythonInterp 3 REQUIRED) -find_package(PythonLibs 3 REQUIRED) +find_package(pybind11 CONFIG REQUIRED) +find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) -if(PYTHONLIBS_FOUND) - message(STATUS "Using Python Libraries at: " ${PYTHON_LIBRARIES}) - message(STATUS "Using Python include directories at: " ${PYTHON_INCLUDE_DIRS}) -else() - message(WARNING "Could not find Python Libraries") -endif() +# Find pybind11 -find_package(Eigen3 REQUIRED) +message([MAIN] "Found pybind11 v${pybind11_VERSION}: ${pybind11_INCLUDE_DIRS}") +message([MAIN] "pybind11_INCLUDE_DIRS = ${pybind11_INCLUDE_DIRS}") +message([MAIN] "pybind11_LIBRARIES = ${pybind11_LIBRARIES}") -find_package(catkin REQUIRED - roscpp - rospy - tf - tf_conversions - sensor_msgs +# Find ROS 2 dependencies +find_package(message_filters REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(grid_map_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(elevation_map_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(image_transport REQUIRED) +find_package(pcl_ros REQUIRED) + + +# List dependencies for ament_target_dependencies +set(dependencies + rclcpp + rclpy std_msgs + std_srvs + builtin_interfaces geometry_msgs + sensor_msgs elevation_map_msgs grid_map_msgs grid_map_ros image_transport pcl_ros - pybind11_catkin -) - -catkin_package( - INCLUDE_DIRS - include - ${EIGEN3_INCLUDE_DIRS} - CATKIN_DEPENDS - roscpp - rospy - tf - tf_conversions - sensor_msgs - std_msgs - geometry_msgs - elevation_map_msgs - grid_map_ros - image_transport - pcl_ros - pybind11_catkin + message_filters ) +# Include directories include_directories( include ${PYTHON_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} + ${Eigen3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} + ${pybind11_INCLUDE_DIRS} ) +# Declare C++ library add_library(elevation_mapping_ros src/elevation_mapping_wrapper.cpp - src/elevation_mapping_ros.cpp) + src/elevation_mapping_ros.cpp +) -target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +# Link the library with necessary dependencies +target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES}) +ament_target_dependencies(elevation_mapping_ros ${dependencies}) +# Declare C++ executable add_executable(elevation_mapping_node src/elevation_mapping_node.cpp) -target_link_libraries(elevation_mapping_node elevation_mapping_ros) -catkin_python_setup() +# Link the executable with the library and dependencies +target_link_libraries(elevation_mapping_node elevation_mapping_ros ${OpenCV_LIBRARIES}) +ament_target_dependencies(elevation_mapping_node ${dependencies}) +# Install targets install( TARGETS elevation_mapping_ros elevation_mapping_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +# Install launch and config directories install( DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + DESTINATION share/${PROJECT_NAME} +) + +# Ament package declaration +ament_package() diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index 0fb75b6a..f631fbba 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -16,26 +16,29 @@ #include // everything needed for embedding // ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include +#include +#include +#include +#include "std_srvs/srv/empty.h" +#include + +#include +#include +#include + +#include +#include // Grid Map -#include -#include +#include +#include #include // PCL @@ -47,145 +50,145 @@ #include #include -#include -#include -#include +#include +#include +#include #include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp" namespace py = pybind11; -namespace elevation_mapping_cupy { - -class ElevationMappingNode { - public: - ElevationMappingNode(ros::NodeHandle& nh); - using RowMatrixXd = Eigen::Matrix; - using ColMatrixXf = Eigen::Matrix; - - using ImageSubscriber = image_transport::SubscriberFilter; - using ImageSubscriberPtr = std::shared_ptr; - - // Subscriber and Synchronizer for CameraInfo messages - using CameraInfoSubscriber = message_filters::Subscriber; - using CameraInfoSubscriberPtr = std::shared_ptr; - using CameraPolicy = message_filters::sync_policies::ApproximateTime; - using CameraSync = message_filters::Synchronizer; - using CameraSyncPtr = std::shared_ptr; - - // Subscriber and Synchronizer for ChannelInfo messages - using ChannelInfoSubscriber = message_filters::Subscriber; - using ChannelInfoSubscriberPtr = std::shared_ptr; - using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime; - using CameraChannelSync = message_filters::Synchronizer; - using CameraChannelSyncPtr = std::shared_ptr; - - // Subscriber and Synchronizer for Pointcloud messages - using PointCloudSubscriber = message_filters::Subscriber; - using PointCloudSubscriberPtr = std::shared_ptr; - using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; - using PointCloudSync = message_filters::Synchronizer; - using PointCloudSyncPtr = std::shared_ptr; - - private: - void readParameters(); - void setupMapPublishers(); - void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key); - void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector& channels); - void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector& channels); - void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key); - void imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); - void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); - // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); - void publishAsPointCloud(const grid_map::GridMap& map) const; - bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); - bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); - bool initializeMap(elevation_map_msgs::Initialize::Request& request, elevation_map_msgs::Initialize::Response& response); - bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); - bool clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); - bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); - void updatePose(const ros::TimerEvent&); - void updateVariance(const ros::TimerEvent&); - void updateTime(const ros::TimerEvent&); - void updateGridMap(const ros::TimerEvent&); - void publishNormalAsArrow(const grid_map::GridMap& map) const; - void initializeWithTF(); - void publishMapToOdom(double error); - void publishStatistics(const ros::TimerEvent&); - void publishMapOfIndex(int index); - - visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; - - ros::NodeHandle nh_; - image_transport::ImageTransport it_; - std::vector pointcloudSubs_; - std::vector imageSubs_; - std::vector cameraInfoSubs_; - std::vector channelInfoSubs_; - std::vector cameraSyncs_; - std::vector cameraChannelSyncs_; - std::vector pointCloudSyncs_; - std::vector mapPubs_; - tf::TransformBroadcaster tfBroadcaster_; - ros::Publisher alivePub_; - ros::Publisher pointPub_; - ros::Publisher normalPub_; - ros::Publisher statisticsPub_; - ros::ServiceServer rawSubmapService_; - ros::ServiceServer clearMapService_; - ros::ServiceServer clearMapWithInitializerService_; - ros::ServiceServer initializeMapService_; - ros::ServiceServer setPublishPointService_; - ros::ServiceServer checkSafetyService_; - ros::Timer updateVarianceTimer_; - ros::Timer updateTimeTimer_; - ros::Timer updatePoseTimer_; - ros::Timer updateGridMapTimer_; - ros::Timer publishStatisticsTimer_; - ros::Time lastStatisticsPublishedTime_; - tf::TransformListener transformListener_; - ElevationMappingWrapper map_; - std::string mapFrameId_; - std::string correctedMapFrameId_; - std::string baseFrameId_; - - // map topics info - std::vector> map_topics_; - std::vector> map_layers_; - std::vector> map_basic_layers_; - std::set map_layers_all_; - std::set map_layers_sync_; - std::vector map_fps_; - std::set map_fps_unique_; - std::vector mapTimers_; - std::map> channels_; - - std::vector initialize_frame_id_; - std::vector initialize_tf_offset_; - std::string initializeMethod_; - - Eigen::Vector3d lowpassPosition_; - Eigen::Vector4d lowpassOrientation_; - - std::mutex mapMutex_; // protects gridMap_ - grid_map::GridMap gridMap_; - std::atomic_bool isGridmapUpdated_; // needs to be atomic (read is not protected by mapMutex_) - - std::mutex errorMutex_; // protects positionError_, and orientationError_ - double positionError_; - double orientationError_; - - double positionAlpha_; - double orientationAlpha_; - - double recordableFps_; - std::atomic_bool enablePointCloudPublishing_; - bool enableNormalArrowPublishing_; - bool enableDriftCorrectedTFPublishing_; - bool useInitializerAtStart_; - double initializeTfGridSize_; - bool alwaysClearWithInitializer_; - std::atomic_int pointCloudProcessCounter_; -}; - -} // namespace elevation_mapping_cupy +// namespace elevation_mapping_cupy { + +// class ElevationMappingNode { +// public: +// ElevationMappingNode(ros::NodeHandle& nh); +// using RowMatrixXd = Eigen::Matrix; +// using ColMatrixXf = Eigen::Matrix; + +// using ImageSubscriber = image_transport::SubscriberFilter; +// using ImageSubscriberPtr = std::shared_ptr; + +// // Subscriber and Synchronizer for CameraInfo messages +// using CameraInfoSubscriber = message_filters::Subscriber; +// using CameraInfoSubscriberPtr = std::shared_ptr; +// using CameraPolicy = message_filters::sync_policies::ApproximateTime; +// using CameraSync = message_filters::Synchronizer; +// using CameraSyncPtr = std::shared_ptr; + +// // Subscriber and Synchronizer for ChannelInfo messages +// using ChannelInfoSubscriber = message_filters::Subscriber; +// using ChannelInfoSubscriberPtr = std::shared_ptr; +// using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime; +// using CameraChannelSync = message_filters::Synchronizer; +// using CameraChannelSyncPtr = std::shared_ptr; + +// // Subscriber and Synchronizer for Pointcloud messages +// using PointCloudSubscriber = message_filters::Subscriber; +// using PointCloudSubscriberPtr = std::shared_ptr; +// using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; +// using PointCloudSync = message_filters::Synchronizer; +// using PointCloudSyncPtr = std::shared_ptr; + +// private: +// void readParameters(); +// void setupMapPublishers(); +// void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key); +// void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector& channels); +// void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector& channels); +// void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key); +// void imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); +// void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); +// // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); +// void publishAsPointCloud(const grid_map::GridMap& map) const; +// bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); +// bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); +// bool initializeMap(elevation_map_msgs::Initialize::Request& request, elevation_map_msgs::Initialize::Response& response); +// bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); +// bool clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); +// bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); +// void updatePose(const ros::TimerEvent&); +// void updateVariance(const ros::TimerEvent&); +// void updateTime(const ros::TimerEvent&); +// void updateGridMap(const ros::TimerEvent&); +// void publishNormalAsArrow(const grid_map::GridMap& map) const; +// void initializeWithTF(); +// void publishMapToOdom(double error); +// void publishStatistics(const ros::TimerEvent&); +// void publishMapOfIndex(int index); + +// visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; + +// ros::NodeHandle nh_; +// image_transport::ImageTransport it_; +// std::vector pointcloudSubs_; +// std::vector imageSubs_; +// std::vector cameraInfoSubs_; +// std::vector channelInfoSubs_; +// std::vector cameraSyncs_; +// std::vector cameraChannelSyncs_; +// std::vector pointCloudSyncs_; +// std::vector mapPubs_; +// tf::TransformBroadcaster tfBroadcaster_; +// ros::Publisher alivePub_; +// ros::Publisher pointPub_; +// ros::Publisher normalPub_; +// ros::Publisher statisticsPub_; +// ros::ServiceServer rawSubmapService_; +// ros::ServiceServer clearMapService_; +// ros::ServiceServer clearMapWithInitializerService_; +// ros::ServiceServer initializeMapService_; +// ros::ServiceServer setPublishPointService_; +// ros::ServiceServer checkSafetyService_; +// ros::Timer updateVarianceTimer_; +// ros::Timer updateTimeTimer_; +// ros::Timer updatePoseTimer_; +// ros::Timer updateGridMapTimer_; +// ros::Timer publishStatisticsTimer_; +// ros::Time lastStatisticsPublishedTime_; +// tf::TransformListener transformListener_; +// ElevationMappingWrapper map_; +// std::string mapFrameId_; +// std::string correctedMapFrameId_; +// std::string baseFrameId_; + +// // map topics info +// std::vector> map_topics_; +// std::vector> map_layers_; +// std::vector> map_basic_layers_; +// std::set map_layers_all_; +// std::set map_layers_sync_; +// std::vector map_fps_; +// std::set map_fps_unique_; +// std::vector mapTimers_; +// std::map> channels_; + +// std::vector initialize_frame_id_; +// std::vector initialize_tf_offset_; +// std::string initializeMethod_; + +// Eigen::Vector3d lowpassPosition_; +// Eigen::Vector4d lowpassOrientation_; + +// std::mutex mapMutex_; // protects gridMap_ +// grid_map::GridMap gridMap_; +// std::atomic_bool isGridmapUpdated_; // needs to be atomic (read is not protected by mapMutex_) + +// std::mutex errorMutex_; // protects positionError_, and orientationError_ +// double positionError_; +// double orientationError_; + +// double positionAlpha_; +// double orientationAlpha_; + +// double recordableFps_; +// std::atomic_bool enablePointCloudPublishing_; +// bool enableNormalArrowPublishing_; +// bool enableDriftCorrectedTFPublishing_; +// bool useInitializerAtStart_; +// double initializeTfGridSize_; +// bool alwaysClearWithInitializer_; +// std::atomic_int pointCloudProcessCounter_; +// }; + +// } // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index 712ed10a..92370e3c 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -15,16 +15,17 @@ #include // everything needed for embedding #include -// ROS -#include -#include -#include -#include -#include +// ROS2 +#include +#include +#include +#include +#include +#include // Grid Map -#include -#include +#include +#include #include // PCL @@ -44,7 +45,7 @@ class ElevationMappingWrapper { ElevationMappingWrapper(); - void initialize(ros::NodeHandle& nh); + void initialize(rclcpp::Node::SharedPtr node); void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); @@ -64,7 +65,7 @@ class ElevationMappingWrapper { void addNormalColorLayer(grid_map::GridMap& map); private: - void setParameters(ros::NodeHandle& nh); + void setParameters(rclcpp::Node::SharedPtr node); py::object map_; py::object param_; double resolution_; diff --git a/elevation_mapping_cupy/package.xml b/elevation_mapping_cupy/package.xml index 9befc801..9cb9c1bb 100644 --- a/elevation_mapping_cupy/package.xml +++ b/elevation_mapping_cupy/package.xml @@ -1,5 +1,5 @@ - + elevation_mapping_cupy 2.0.0 The elevation mapping on GPU @@ -8,19 +8,23 @@ MIT - catkin - roscpp - rospy - tf - tf_conversions - sensor_msgs + ament_cmake + rclcpp std_msgs + std_srvs + sensor_msgs geometry_msgs + message_filters grid_map_msgs elevation_map_msgs grid_map_ros image_transport pcl_ros - pybind11_catkin + pybind11 + ament_lint_auto + ament_lint_common - + + ament_cmake + + \ No newline at end of file diff --git a/elevation_mapping_cupy/src/elevation_mapping_node.cpp b/elevation_mapping_cupy/src/elevation_mapping_node.cpp index 3d7a778f..05fac5d6 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_node.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_node.cpp @@ -6,22 +6,22 @@ // Pybind #include // everything needed for embedding -// ROS -#include - +// ROS2 +#include #include "elevation_mapping_cupy/elevation_mapping_ros.hpp" int main(int argc, char** argv) { - ros::init(argc, argv, "elevation_mapping"); - ros::NodeHandle nh("~"); + rclcpp::init(argc, argv); + auto node = std::make_shared("elevation_mapping"); py::scoped_interpreter guard{}; // start the interpreter and keep it alive - elevation_mapping_cupy::ElevationMappingNode mapNode(nh); + // elevation_mapping_cupy::ElevationMappingNode mapNode(node); py::gil_scoped_release release; // Spin - ros::AsyncSpinner spinner(1); // Use n threads - spinner.start(); - ros::waitForShutdown(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); return 0; -} +} \ No newline at end of file diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 2fbc3128..374bf709 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -1,820 +1,820 @@ -// -// Copyright (c) 2022, Takahiro Miki. All rights reserved. -// Licensed under the MIT license. See LICENSE file in the project root for details. -// - -#include "elevation_mapping_cupy/elevation_mapping_ros.hpp" - -// Pybind -#include - -// ROS -#include -#include -#include - -// PCL -#include - -#include - -namespace elevation_mapping_cupy { - -ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) - : it_(nh), - lowpassPosition_(0, 0, 0), - lowpassOrientation_(0, 0, 0, 1), - positionError_(0), - orientationError_(0), - positionAlpha_(0.1), - orientationAlpha_(0.1), - enablePointCloudPublishing_(false), - isGridmapUpdated_(false) { - nh_ = nh; - - std::string pose_topic, map_frame; - XmlRpc::XmlRpcValue publishers; - XmlRpc::XmlRpcValue subscribers; - std::vector map_topics; - double recordableFps, updateVarianceFps, timeInterval, updatePoseFps, updateGridMapFps, publishStatisticsFps; - bool enablePointCloudPublishing(false); - - // Read parameters - nh.getParam("subscribers", subscribers); - nh.getParam("publishers", publishers); - if (!subscribers.valid()) { - ROS_FATAL("There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); - } - if (!publishers.valid()) { - ROS_FATAL("There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit"); - } - nh.param>("initialize_frame_id", initialize_frame_id_, {"base"}); - nh.param>("initialize_tf_offset", initialize_tf_offset_, {0.0}); - nh.param("pose_topic", pose_topic, "pose"); - nh.param("map_frame", mapFrameId_, "map"); - nh.param("base_frame", baseFrameId_, "base"); - nh.param("corrected_map_frame", correctedMapFrameId_, "corrected_map"); - nh.param("initialize_method", initializeMethod_, "cubic"); - nh.param("position_lowpass_alpha", positionAlpha_, 0.2); - nh.param("orientation_lowpass_alpha", orientationAlpha_, 0.2); - nh.param("recordable_fps", recordableFps, 3.0); - nh.param("update_variance_fps", updateVarianceFps, 1.0); - nh.param("time_interval", timeInterval, 0.1); - nh.param("update_pose_fps", updatePoseFps, 10.0); - nh.param("initialize_tf_grid_size", initializeTfGridSize_, 0.5); - nh.param("map_acquire_fps", updateGridMapFps, 5.0); - nh.param("publish_statistics_fps", publishStatisticsFps, 1.0); - nh.param("enable_pointcloud_publishing", enablePointCloudPublishing, false); - nh.param("enable_normal_arrow_publishing", enableNormalArrowPublishing_, false); - nh.param("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_, false); - nh.param("use_initializer_at_start", useInitializerAtStart_, false); - nh.param("always_clear_with_initializer", alwaysClearWithInitializer_, false); - - enablePointCloudPublishing_ = enablePointCloudPublishing; - - // Iterate all the subscribers - // here we have to remove all the stuff - for (auto& subscriber : subscribers) { - std::string key = subscriber.first; - auto type = static_cast(subscriber.second["data_type"]); - - // Initialize subscribers depending on the type - if (type == "pointcloud") { - std::string pointcloud_topic = subscriber.second["topic_name"]; - channels_[key].push_back("x"); - channels_[key].push_back("y"); - channels_[key].push_back("z"); - boost::function f = boost::bind(&ElevationMappingNode::pointcloudCallback, this, _1, key); - ros::Subscriber sub = nh_.subscribe(pointcloud_topic, 1, f); - pointcloudSubs_.push_back(sub); - ROS_INFO_STREAM("Subscribed to PointCloud2 topic: " << pointcloud_topic); - } - else if (type == "image") { - std::string camera_topic = subscriber.second["topic_name"]; - std::string info_topic = subscriber.second["camera_info_topic_name"]; - - // Handle compressed images with transport hints - // We obtain the hint from the last part of the topic name - std::string transport_hint = "compressed"; - std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name - if (ind != std::string::npos) { - transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part - camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic - } else { - transport_hint = "raw"; // In the default case we assume raw topic - } - - // Setup subscriber - const auto hint = image_transport::TransportHints(transport_hint, ros::TransportHints(), ros::NodeHandle(camera_topic)); - ImageSubscriberPtr image_sub = std::make_shared(); - image_sub->subscribe(it_, camera_topic, 1, hint); - imageSubs_.push_back(image_sub); - - CameraInfoSubscriberPtr cam_info_sub = std::make_shared(); - cam_info_sub->subscribe(nh_, info_topic, 1); - cameraInfoSubs_.push_back(cam_info_sub); - - std::string channel_info_topic; - // If there is channel info topic setting, we use it. - if (subscriber.second.hasMember("channel_info_topic_name")) { - std::string channel_info_topic = subscriber.second["channel_info_topic_name"]; - ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(); - channel_info_sub->subscribe(nh_, channel_info_topic, 1); - channelInfoSubs_.push_back(channel_info_sub); - CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); - sync->registerCallback(boost::bind(&ElevationMappingNode::imageChannelCallback, this, _1, _2, _3)); - cameraChannelSyncs_.push_back(sync); - ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ", Channel info topic: " << channel_info_topic); - } - else { - // If there is channels setting, we use it. Otherwise, we use rgb as default. - if (subscriber.second.hasMember("channels")) { - const auto& channels = subscriber.second["channels"]; - for (int32_t i = 0; i < channels.size(); ++i) { - auto elem = static_cast(channels[i]); - channels_[key].push_back(elem); - } - } - else { - channels_[key].push_back("rgb"); - } - ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ". Channel info topic: " << (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")) : channel_info_topic)); - CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); - sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2, key)); - cameraSyncs_.push_back(sync); - } - - - } else { - ROS_WARN_STREAM("Subscriber data_type [" << type << "] Not valid. Supported types: pointcloud, image"); - continue; - } - } - - map_.initialize(nh_); - - // Register map publishers - for (auto itr = publishers.begin(); itr != publishers.end(); ++itr) { - // Parse params - std::string topic_name = itr->first; - std::vector layers_list; - std::vector basic_layers_list; - auto layers = itr->second["layers"]; - auto basic_layers = itr->second["basic_layers"]; - double fps = itr->second["fps"]; - - if (fps > updateGridMapFps) { - ROS_WARN( - "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " - "fps.", - topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); - } - - for (int32_t i = 0; i < layers.size(); ++i) { - layers_list.push_back(static_cast(layers[i])); - } - - for (int32_t i = 0; i < basic_layers.size(); ++i) { - basic_layers_list.push_back(static_cast(basic_layers[i])); - } - - // Make publishers - ros::Publisher pub = nh_.advertise(topic_name, 1); - mapPubs_.push_back(pub); - - // Register map layers - map_layers_.push_back(layers_list); - map_basic_layers_.push_back(basic_layers_list); - - // Register map fps - map_fps_.push_back(fps); - map_fps_unique_.insert(fps); - } - setupMapPublishers(); - - pointPub_ = nh_.advertise("elevation_map_points", 1); - alivePub_ = nh_.advertise("alive", 1); - normalPub_ = nh_.advertise("normal", 1); - statisticsPub_ = nh_.advertise("statistics", 1); - - gridMap_.setFrameId(mapFrameId_); - rawSubmapService_ = nh_.advertiseService("get_raw_submap", &ElevationMappingNode::getSubmap, this); - clearMapService_ = nh_.advertiseService("clear_map", &ElevationMappingNode::clearMap, this); - initializeMapService_ = nh_.advertiseService("initialize", &ElevationMappingNode::initializeMap, this); - clearMapWithInitializerService_ = - nh_.advertiseService("clear_map_with_initializer", &ElevationMappingNode::clearMapWithInitializer, this); - setPublishPointService_ = nh_.advertiseService("set_publish_points", &ElevationMappingNode::setPublishPoint, this); - checkSafetyService_ = nh_.advertiseService("check_safety", &ElevationMappingNode::checkSafety, this); - - if (updateVarianceFps > 0) { - double duration = 1.0 / (updateVarianceFps + 0.00001); - updateVarianceTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateVariance, this, false, true); - } - if (timeInterval > 0) { - double duration = timeInterval; - updateTimeTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateTime, this, false, true); - } - if (updatePoseFps > 0) { - double duration = 1.0 / (updatePoseFps + 0.00001); - updatePoseTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updatePose, this, false, true); - } - if (updateGridMapFps > 0) { - double duration = 1.0 / (updateGridMapFps + 0.00001); - updateGridMapTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateGridMap, this, false, true); - } - if (publishStatisticsFps > 0) { - double duration = 1.0 / (publishStatisticsFps + 0.00001); - publishStatisticsTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::publishStatistics, this, false, true); - } - lastStatisticsPublishedTime_ = ros::Time::now(); - ROS_INFO("[ElevationMappingCupy] finish initialization"); -} - -// Setup map publishers -void ElevationMappingNode::setupMapPublishers() { - // Find the layers with highest fps. - float max_fps = -1; - // Create timers for each unique map frequencies - for (auto fps : map_fps_unique_) { - // Which publisher to call in the timer callback - std::vector indices; - // If this fps is max, update the map layers. - if (fps >= max_fps) { - max_fps = fps; - map_layers_all_.clear(); - } - for (int i = 0; i < map_fps_.size(); i++) { - if (map_fps_[i] == fps) { - indices.push_back(i); - // If this fps is max, add layers - if (fps >= max_fps) { - for (const auto layer : map_layers_[i]) { - map_layers_all_.insert(layer); - } - } - } - } - // Callback funtion. - // It publishes to specific topics. - auto cb = [this, indices](const ros::TimerEvent&) { - for (int i : indices) { - publishMapOfIndex(i); - } - }; - double duration = 1.0 / (fps + 0.00001); - mapTimers_.push_back(nh_.createTimer(ros::Duration(duration), cb)); - } -} - -void ElevationMappingNode::publishMapOfIndex(int index) { - // publish the map layers of index - if (!isGridmapUpdated_) { - return; - } - grid_map_msgs::GridMap msg; - std::vector layers; - - { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in - // map_layers_all_ - std::lock_guard lock(mapMutex_); - for (const auto& layer : map_layers_[index]) { - const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); - if (is_layer_in_all && gridMap_.exists(layer)) { - layers.push_back(layer); - } else if (map_.exists_layer(layer)) { - // if there are layers which is not in the syncing layer. - ElevationMappingWrapper::RowMatrixXf map_data; - map_.get_layer_data(layer, map_data); - gridMap_.add(layer, map_data); - layers.push_back(layer); - } - } - if (layers.empty()) { - return; - } - - grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg); - } - - msg.basic_layers = map_basic_layers_[index]; - mapPubs_[index].publish(msg); -} - -void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key) { - - // get channels - auto fields = cloud.fields; - std::vector channels; - - for (int it = 0; it < fields.size(); it++) { - auto& field = fields[it]; - channels.push_back(field.name); - } - inputPointCloud(cloud, channels); - - // This is used for publishing as statistics. - pointCloudProcessCounter_++; -} - -void ElevationMappingNode::inputPointCloud(const sensor_msgs::PointCloud2& cloud, - const std::vector& channels) { - auto start = ros::Time::now(); - auto* pcl_pc = new pcl::PCLPointCloud2; - pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc); - pcl_conversions::toPCL(cloud, *pcl_pc); - - // get channels - auto fields = cloud.fields; - uint array_dim = channels.size(); - - RowMatrixXd points = RowMatrixXd(pcl_pc->width * pcl_pc->height, array_dim); - - for (unsigned int i = 0; i < pcl_pc->width * pcl_pc->height; ++i) { - for (unsigned int j = 0; j < channels.size(); ++j) { - float temp; - uint point_idx = i * pcl_pc->point_step + pcl_pc->fields[j].offset; - memcpy(&temp, &pcl_pc->data[point_idx], sizeof(float)); - points(i, j) = static_cast(temp); - } - } - // get pose of sensor in map frame - tf::StampedTransform transformTf; - std::string sensorFrameId = cloud.header.frame_id; - auto timeStamp = cloud.header.stamp; - Eigen::Affine3d transformationSensorToMap; - try { - transformListener_.waitForTransform(mapFrameId_, sensorFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationSensorToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return; - } - - double positionError{0.0}; - double orientationError{0.0}; - { - std::lock_guard lock(errorMutex_); - positionError = positionError_; - orientationError = orientationError_; - } - map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, - orientationError); - - if (enableDriftCorrectedTFPublishing_) { - publishMapToOdom(map_.get_additive_mean_error()); - } - - ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), - (ros::Time::now() - start).toSec()); - ROS_DEBUG_THROTTLE(1.0, "positionError: %f ", positionError); - ROS_DEBUG_THROTTLE(1.0, "orientationError: %f ", orientationError); - -} - -void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const std::vector& channels) { - // Get image - cv::Mat image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; - - // Change encoding to RGB/RGBA - if (image_msg->encoding == "bgr8") { - cv::cvtColor(image, image, CV_BGR2RGB); - } else if (image_msg->encoding == "bgra8") { - cv::cvtColor(image, image, CV_BGRA2RGBA); - } - - // Extract camera matrix - Eigen::Map> cameraMatrix(&camera_info_msg->K[0]); - - // Extract distortion coefficients - Eigen::VectorXd distortionCoeffs; - if (!camera_info_msg->D.empty()) { - distortionCoeffs = Eigen::Map(camera_info_msg->D.data(), camera_info_msg->D.size()); - } else { - ROS_WARN("Distortion coefficients are empty."); - distortionCoeffs = Eigen::VectorXd::Zero(5); - // return; - } - - // distortion model - std::string distortion_model = camera_info_msg->distortion_model; +// // +// // Copyright (c) 2022, Takahiro Miki. All rights reserved. +// // Licensed under the MIT license. See LICENSE file in the project root for details. +// // + +// #include "elevation_mapping_cupy/elevation_mapping_ros.hpp" + +// // Pybind +// #include + +// // ROS +// #include +// #include +// #include + +// // PCL +// #include + +// #include + +// namespace elevation_mapping_cupy { + +// ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) +// : it_(nh), +// lowpassPosition_(0, 0, 0), +// lowpassOrientation_(0, 0, 0, 1), +// positionError_(0), +// orientationError_(0), +// positionAlpha_(0.1), +// orientationAlpha_(0.1), +// enablePointCloudPublishing_(false), +// isGridmapUpdated_(false) { +// nh_ = nh; + +// std::string pose_topic, map_frame; +// XmlRpc::XmlRpcValue publishers; +// XmlRpc::XmlRpcValue subscribers; +// std::vector map_topics; +// double recordableFps, updateVarianceFps, timeInterval, updatePoseFps, updateGridMapFps, publishStatisticsFps; +// bool enablePointCloudPublishing(false); + +// // Read parameters +// nh.getParam("subscribers", subscribers); +// nh.getParam("publishers", publishers); +// if (!subscribers.valid()) { +// ROS_FATAL("There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); +// } +// if (!publishers.valid()) { +// ROS_FATAL("There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit"); +// } +// nh.param>("initialize_frame_id", initialize_frame_id_, {"base"}); +// nh.param>("initialize_tf_offset", initialize_tf_offset_, {0.0}); +// nh.param("pose_topic", pose_topic, "pose"); +// nh.param("map_frame", mapFrameId_, "map"); +// nh.param("base_frame", baseFrameId_, "base"); +// nh.param("corrected_map_frame", correctedMapFrameId_, "corrected_map"); +// nh.param("initialize_method", initializeMethod_, "cubic"); +// nh.param("position_lowpass_alpha", positionAlpha_, 0.2); +// nh.param("orientation_lowpass_alpha", orientationAlpha_, 0.2); +// nh.param("recordable_fps", recordableFps, 3.0); +// nh.param("update_variance_fps", updateVarianceFps, 1.0); +// nh.param("time_interval", timeInterval, 0.1); +// nh.param("update_pose_fps", updatePoseFps, 10.0); +// nh.param("initialize_tf_grid_size", initializeTfGridSize_, 0.5); +// nh.param("map_acquire_fps", updateGridMapFps, 5.0); +// nh.param("publish_statistics_fps", publishStatisticsFps, 1.0); +// nh.param("enable_pointcloud_publishing", enablePointCloudPublishing, false); +// nh.param("enable_normal_arrow_publishing", enableNormalArrowPublishing_, false); +// nh.param("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_, false); +// nh.param("use_initializer_at_start", useInitializerAtStart_, false); +// nh.param("always_clear_with_initializer", alwaysClearWithInitializer_, false); + +// enablePointCloudPublishing_ = enablePointCloudPublishing; + +// // Iterate all the subscribers +// // here we have to remove all the stuff +// for (auto& subscriber : subscribers) { +// std::string key = subscriber.first; +// auto type = static_cast(subscriber.second["data_type"]); + +// // Initialize subscribers depending on the type +// if (type == "pointcloud") { +// std::string pointcloud_topic = subscriber.second["topic_name"]; +// channels_[key].push_back("x"); +// channels_[key].push_back("y"); +// channels_[key].push_back("z"); +// boost::function f = boost::bind(&ElevationMappingNode::pointcloudCallback, this, _1, key); +// ros::Subscriber sub = nh_.subscribe(pointcloud_topic, 1, f); +// pointcloudSubs_.push_back(sub); +// ROS_INFO_STREAM("Subscribed to PointCloud2 topic: " << pointcloud_topic); +// } +// else if (type == "image") { +// std::string camera_topic = subscriber.second["topic_name"]; +// std::string info_topic = subscriber.second["camera_info_topic_name"]; + +// // Handle compressed images with transport hints +// // We obtain the hint from the last part of the topic name +// std::string transport_hint = "compressed"; +// std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name +// if (ind != std::string::npos) { +// transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part +// camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic +// } else { +// transport_hint = "raw"; // In the default case we assume raw topic +// } + +// // Setup subscriber +// const auto hint = image_transport::TransportHints(transport_hint, ros::TransportHints(), ros::NodeHandle(camera_topic)); +// ImageSubscriberPtr image_sub = std::make_shared(); +// image_sub->subscribe(it_, camera_topic, 1, hint); +// imageSubs_.push_back(image_sub); + +// CameraInfoSubscriberPtr cam_info_sub = std::make_shared(); +// cam_info_sub->subscribe(nh_, info_topic, 1); +// cameraInfoSubs_.push_back(cam_info_sub); + +// std::string channel_info_topic; +// // If there is channel info topic setting, we use it. +// if (subscriber.second.hasMember("channel_info_topic_name")) { +// std::string channel_info_topic = subscriber.second["channel_info_topic_name"]; +// ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(); +// channel_info_sub->subscribe(nh_, channel_info_topic, 1); +// channelInfoSubs_.push_back(channel_info_sub); +// CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); +// sync->registerCallback(boost::bind(&ElevationMappingNode::imageChannelCallback, this, _1, _2, _3)); +// cameraChannelSyncs_.push_back(sync); +// ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ", Channel info topic: " << channel_info_topic); +// } +// else { +// // If there is channels setting, we use it. Otherwise, we use rgb as default. +// if (subscriber.second.hasMember("channels")) { +// const auto& channels = subscriber.second["channels"]; +// for (int32_t i = 0; i < channels.size(); ++i) { +// auto elem = static_cast(channels[i]); +// channels_[key].push_back(elem); +// } +// } +// else { +// channels_[key].push_back("rgb"); +// } +// ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ". Channel info topic: " << (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")) : channel_info_topic)); +// CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); +// sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2, key)); +// cameraSyncs_.push_back(sync); +// } + + +// } else { +// ROS_WARN_STREAM("Subscriber data_type [" << type << "] Not valid. Supported types: pointcloud, image"); +// continue; +// } +// } + +// map_.initialize(nh_); + +// // Register map publishers +// for (auto itr = publishers.begin(); itr != publishers.end(); ++itr) { +// // Parse params +// std::string topic_name = itr->first; +// std::vector layers_list; +// std::vector basic_layers_list; +// auto layers = itr->second["layers"]; +// auto basic_layers = itr->second["basic_layers"]; +// double fps = itr->second["fps"]; + +// if (fps > updateGridMapFps) { +// ROS_WARN( +// "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " +// "fps.", +// topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); +// } + +// for (int32_t i = 0; i < layers.size(); ++i) { +// layers_list.push_back(static_cast(layers[i])); +// } + +// for (int32_t i = 0; i < basic_layers.size(); ++i) { +// basic_layers_list.push_back(static_cast(basic_layers[i])); +// } + +// // Make publishers +// ros::Publisher pub = nh_.advertise(topic_name, 1); +// mapPubs_.push_back(pub); + +// // Register map layers +// map_layers_.push_back(layers_list); +// map_basic_layers_.push_back(basic_layers_list); + +// // Register map fps +// map_fps_.push_back(fps); +// map_fps_unique_.insert(fps); +// } +// setupMapPublishers(); + +// pointPub_ = nh_.advertise("elevation_map_points", 1); +// alivePub_ = nh_.advertise("alive", 1); +// normalPub_ = nh_.advertise("normal", 1); +// statisticsPub_ = nh_.advertise("statistics", 1); + +// gridMap_.setFrameId(mapFrameId_); +// rawSubmapService_ = nh_.advertiseService("get_raw_submap", &ElevationMappingNode::getSubmap, this); +// clearMapService_ = nh_.advertiseService("clear_map", &ElevationMappingNode::clearMap, this); +// initializeMapService_ = nh_.advertiseService("initialize", &ElevationMappingNode::initializeMap, this); +// clearMapWithInitializerService_ = +// nh_.advertiseService("clear_map_with_initializer", &ElevationMappingNode::clearMapWithInitializer, this); +// setPublishPointService_ = nh_.advertiseService("set_publish_points", &ElevationMappingNode::setPublishPoint, this); +// checkSafetyService_ = nh_.advertiseService("check_safety", &ElevationMappingNode::checkSafety, this); + +// if (updateVarianceFps > 0) { +// double duration = 1.0 / (updateVarianceFps + 0.00001); +// updateVarianceTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateVariance, this, false, true); +// } +// if (timeInterval > 0) { +// double duration = timeInterval; +// updateTimeTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateTime, this, false, true); +// } +// if (updatePoseFps > 0) { +// double duration = 1.0 / (updatePoseFps + 0.00001); +// updatePoseTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updatePose, this, false, true); +// } +// if (updateGridMapFps > 0) { +// double duration = 1.0 / (updateGridMapFps + 0.00001); +// updateGridMapTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateGridMap, this, false, true); +// } +// if (publishStatisticsFps > 0) { +// double duration = 1.0 / (publishStatisticsFps + 0.00001); +// publishStatisticsTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::publishStatistics, this, false, true); +// } +// lastStatisticsPublishedTime_ = ros::Time::now(); +// ROS_INFO("[ElevationMappingCupy] finish initialization"); +// } + +// // Setup map publishers +// void ElevationMappingNode::setupMapPublishers() { +// // Find the layers with highest fps. +// float max_fps = -1; +// // Create timers for each unique map frequencies +// for (auto fps : map_fps_unique_) { +// // Which publisher to call in the timer callback +// std::vector indices; +// // If this fps is max, update the map layers. +// if (fps >= max_fps) { +// max_fps = fps; +// map_layers_all_.clear(); +// } +// for (int i = 0; i < map_fps_.size(); i++) { +// if (map_fps_[i] == fps) { +// indices.push_back(i); +// // If this fps is max, add layers +// if (fps >= max_fps) { +// for (const auto layer : map_layers_[i]) { +// map_layers_all_.insert(layer); +// } +// } +// } +// } +// // Callback funtion. +// // It publishes to specific topics. +// auto cb = [this, indices](const ros::TimerEvent&) { +// for (int i : indices) { +// publishMapOfIndex(i); +// } +// }; +// double duration = 1.0 / (fps + 0.00001); +// mapTimers_.push_back(nh_.createTimer(ros::Duration(duration), cb)); +// } +// } + +// void ElevationMappingNode::publishMapOfIndex(int index) { +// // publish the map layers of index +// if (!isGridmapUpdated_) { +// return; +// } +// grid_map_msgs::GridMap msg; +// std::vector layers; + +// { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in +// // map_layers_all_ +// std::lock_guard lock(mapMutex_); +// for (const auto& layer : map_layers_[index]) { +// const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); +// if (is_layer_in_all && gridMap_.exists(layer)) { +// layers.push_back(layer); +// } else if (map_.exists_layer(layer)) { +// // if there are layers which is not in the syncing layer. +// ElevationMappingWrapper::RowMatrixXf map_data; +// map_.get_layer_data(layer, map_data); +// gridMap_.add(layer, map_data); +// layers.push_back(layer); +// } +// } +// if (layers.empty()) { +// return; +// } + +// grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg); +// } + +// msg.basic_layers = map_basic_layers_[index]; +// mapPubs_[index].publish(msg); +// } + +// void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key) { + +// // get channels +// auto fields = cloud.fields; +// std::vector channels; + +// for (int it = 0; it < fields.size(); it++) { +// auto& field = fields[it]; +// channels.push_back(field.name); +// } +// inputPointCloud(cloud, channels); + +// // This is used for publishing as statistics. +// pointCloudProcessCounter_++; +// } + +// void ElevationMappingNode::inputPointCloud(const sensor_msgs::PointCloud2& cloud, +// const std::vector& channels) { +// auto start = ros::Time::now(); +// auto* pcl_pc = new pcl::PCLPointCloud2; +// pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc); +// pcl_conversions::toPCL(cloud, *pcl_pc); + +// // get channels +// auto fields = cloud.fields; +// uint array_dim = channels.size(); + +// RowMatrixXd points = RowMatrixXd(pcl_pc->width * pcl_pc->height, array_dim); + +// for (unsigned int i = 0; i < pcl_pc->width * pcl_pc->height; ++i) { +// for (unsigned int j = 0; j < channels.size(); ++j) { +// float temp; +// uint point_idx = i * pcl_pc->point_step + pcl_pc->fields[j].offset; +// memcpy(&temp, &pcl_pc->data[point_idx], sizeof(float)); +// points(i, j) = static_cast(temp); +// } +// } +// // get pose of sensor in map frame +// tf::StampedTransform transformTf; +// std::string sensorFrameId = cloud.header.frame_id; +// auto timeStamp = cloud.header.stamp; +// Eigen::Affine3d transformationSensorToMap; +// try { +// transformListener_.waitForTransform(mapFrameId_, sensorFrameId, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationSensorToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return; +// } + +// double positionError{0.0}; +// double orientationError{0.0}; +// { +// std::lock_guard lock(errorMutex_); +// positionError = positionError_; +// orientationError = orientationError_; +// } +// map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, +// orientationError); + +// if (enableDriftCorrectedTFPublishing_) { +// publishMapToOdom(map_.get_additive_mean_error()); +// } + +// ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), +// (ros::Time::now() - start).toSec()); +// ROS_DEBUG_THROTTLE(1.0, "positionError: %f ", positionError); +// ROS_DEBUG_THROTTLE(1.0, "orientationError: %f ", orientationError); + +// } + +// void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_msg, +// const sensor_msgs::CameraInfoConstPtr& camera_info_msg, +// const std::vector& channels) { +// // Get image +// cv::Mat image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; + +// // Change encoding to RGB/RGBA +// if (image_msg->encoding == "bgr8") { +// cv::cvtColor(image, image, CV_BGR2RGB); +// } else if (image_msg->encoding == "bgra8") { +// cv::cvtColor(image, image, CV_BGRA2RGBA); +// } + +// // Extract camera matrix +// Eigen::Map> cameraMatrix(&camera_info_msg->K[0]); + +// // Extract distortion coefficients +// Eigen::VectorXd distortionCoeffs; +// if (!camera_info_msg->D.empty()) { +// distortionCoeffs = Eigen::Map(camera_info_msg->D.data(), camera_info_msg->D.size()); +// } else { +// ROS_WARN("Distortion coefficients are empty."); +// distortionCoeffs = Eigen::VectorXd::Zero(5); +// // return; +// } + +// // distortion model +// std::string distortion_model = camera_info_msg->distortion_model; - // Get pose of sensor in map frame - tf::StampedTransform transformTf; - std::string sensorFrameId = image_msg->header.frame_id; - auto timeStamp = image_msg->header.stamp; - Eigen::Affine3d transformationMapToSensor; - try { - transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(sensorFrameId, mapFrameId_, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationMapToSensor); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return; - } - - // Transform image to vector of Eigen matrices for easy pybind conversion - std::vector image_split; - std::vector multichannel_image; - cv::split(image, image_split); - for (auto img : image_split) { - ColMatrixXf eigen_img; - cv::cv2eigen(img, eigen_img); - multichannel_image.push_back(eigen_img); - } - - // Check if the size of multichannel_image and channels and channel_methods matches. "rgb" counts for 3 layers. - int total_channels = 0; - for (const auto& channel : channels) { - if (channel == "rgb") { - total_channels += 3; - } else { - total_channels += 1; - } - } - if (total_channels != multichannel_image.size()) { - ROS_ERROR("Mismatch in the size of multichannel_image (%d), channels (%d). Please check the input.", multichannel_image.size(), channels.size()); - ROS_ERROR_STREAM("Current Channels: " << boost::algorithm::join(channels, ", ")); - return; - } - - // Pass image to pipeline - map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, - distortionCoeffs, distortion_model, image.rows, image.cols); -} - -void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const std::string& key) { - auto start = ros::Time::now(); - inputImage(image_msg, camera_info_msg, channels_[key]); - ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); -} - -void ElevationMappingNode::imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, - const sensor_msgs::CameraInfoConstPtr& camera_info_msg, - const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg) { - auto start = ros::Time::now(); - // Default channels and fusion methods for image is rgb and image_color - std::vector channels; - channels = channel_info_msg->channels; - inputImage(image_msg, camera_info_msg, channels); - ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); -} - -void ElevationMappingNode::updatePose(const ros::TimerEvent&) { - tf::StampedTransform transformTf; - const auto& timeStamp = ros::Time::now(); - Eigen::Affine3d transformationBaseToMap; - try { - transformListener_.waitForTransform(mapFrameId_, baseFrameId_, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, baseFrameId_, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return; - } - - // This is to check if the robot is moving. If the robot is not moving, drift compensation is disabled to avoid creating artifacts. - Eigen::Vector3d position(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); - map_.move_to(position, transformationBaseToMap.rotation().transpose()); - Eigen::Vector3d position3(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); - Eigen::Vector4d orientation(transformTf.getRotation().x(), transformTf.getRotation().y(), transformTf.getRotation().z(), - transformTf.getRotation().w()); - lowpassPosition_ = positionAlpha_ * position3 + (1 - positionAlpha_) * lowpassPosition_; - lowpassOrientation_ = orientationAlpha_ * orientation + (1 - orientationAlpha_) * lowpassOrientation_; - { - std::lock_guard lock(errorMutex_); - positionError_ = (position3 - lowpassPosition_).norm(); - orientationError_ = (orientation - lowpassOrientation_).norm(); - } - - if (useInitializerAtStart_) { - ROS_INFO("Clearing map with initializer."); - initializeWithTF(); - useInitializerAtStart_ = false; - } -} - -void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const { - sensor_msgs::PointCloud2 msg; - grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg); - pointPub_.publish(msg); -} - -bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) { - std::string requestedFrameId = request.frame_id; - Eigen::Isometry3d transformationOdomToMap; - grid_map::Position requestedSubmapPosition(request.position_x, request.position_y); - if (requestedFrameId != mapFrameId_) { - tf::StampedTransform transformTf; - const auto& timeStamp = ros::Time::now(); - try { - transformListener_.waitForTransform(requestedFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(requestedFrameId, mapFrameId_, timeStamp, transformTf); - tf::poseTFToEigen(transformTf, transformationOdomToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return false; - } - Eigen::Vector3d p(request.position_x, request.position_y, 0); - Eigen::Vector3d mapP = transformationOdomToMap.inverse() * p; - requestedSubmapPosition.x() = mapP.x(); - requestedSubmapPosition.y() = mapP.y(); - } - grid_map::Length requestedSubmapLength(request.length_x, request.length_y); - ROS_DEBUG("Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", requestedSubmapPosition.x(), requestedSubmapPosition.y(), - requestedSubmapLength(0), requestedSubmapLength(1)); - - bool isSuccess; - grid_map::Index index; - grid_map::GridMap subMap; - { - std::lock_guard lock(mapMutex_); - subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, index, isSuccess); - } - const auto& length = subMap.getLength(); - if (requestedFrameId != mapFrameId_) { - subMap = subMap.getTransformedMap(transformationOdomToMap, "elevation", requestedFrameId); - } - - if (request.layers.empty()) { - grid_map::GridMapRosConverter::toMessage(subMap, response.map); - } else { - std::vector layers; - for (const auto& layer : request.layers) { - layers.push_back(layer); - } - grid_map::GridMapRosConverter::toMessage(subMap, layers, response.map); - } - return isSuccess; -} - -bool ElevationMappingNode::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { - ROS_INFO("Clearing map."); - map_.clear(); - if (alwaysClearWithInitializer_) { - initializeWithTF(); - } - return true; -} - -bool ElevationMappingNode::clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { - ROS_INFO("Clearing map with initializer."); - map_.clear(); - initializeWithTF(); - return true; -} - -void ElevationMappingNode::initializeWithTF() { - std::vector points; - const auto& timeStamp = ros::Time::now(); - int i = 0; - Eigen::Vector3d p; - for (const auto& frame_id : initialize_frame_id_) { - // Get tf from map frame to tf frame - Eigen::Affine3d transformationBaseToMap; - tf::StampedTransform transformTf; - try { - transformListener_.waitForTransform(mapFrameId_, frame_id, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, frame_id, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return; - } - p = transformationBaseToMap.translation(); - p.z() += initialize_tf_offset_[i]; - points.push_back(p); - i++; - } - if (!points.empty() && points.size() < 3) { - points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, initializeTfGridSize_, 0)); - points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, initializeTfGridSize_, 0)); - points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, -initializeTfGridSize_, 0)); - points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, -initializeTfGridSize_, 0)); - } - ROS_INFO_STREAM("Initializing map with points using " << initializeMethod_); - map_.initializeWithPoints(points, initializeMethod_); -} - -bool ElevationMappingNode::checkSafety(elevation_map_msgs::CheckSafety::Request& request, - elevation_map_msgs::CheckSafety::Response& response) { - for (const auto& polygonstamped : request.polygons) { - if (polygonstamped.polygon.points.empty()) { - continue; - } - std::vector polygon; - std::vector untraversable_polygon; - Eigen::Vector3d result; - result.setZero(); - const auto& polygonFrameId = polygonstamped.header.frame_id; - const auto& timeStamp = polygonstamped.header.stamp; - double polygon_z = polygonstamped.polygon.points[0].z; - - // Get tf from map frame to polygon frame - if (mapFrameId_ != polygonFrameId) { - Eigen::Affine3d transformationBaseToMap; - tf::StampedTransform transformTf; - try { - transformListener_.waitForTransform(mapFrameId_, polygonFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, polygonFrameId, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return false; - } - for (const auto& p : polygonstamped.polygon.points) { - const auto& pvector = Eigen::Vector3d(p.x, p.y, p.z); - const auto transformed_p = transformationBaseToMap * pvector; - polygon.emplace_back(Eigen::Vector2d(transformed_p.x(), transformed_p.y())); - } - } else { - for (const auto& p : polygonstamped.polygon.points) { - polygon.emplace_back(Eigen::Vector2d(p.x, p.y)); - } - } - - map_.get_polygon_traversability(polygon, result, untraversable_polygon); - - geometry_msgs::PolygonStamped untraversable_polygonstamped; - untraversable_polygonstamped.header.stamp = ros::Time::now(); - untraversable_polygonstamped.header.frame_id = mapFrameId_; - for (const auto& p : untraversable_polygon) { - geometry_msgs::Point32 point; - point.x = static_cast(p.x()); - point.y = static_cast(p.y()); - point.z = static_cast(polygon_z); - untraversable_polygonstamped.polygon.points.push_back(point); - } - // traversability_result; - response.is_safe.push_back(bool(result[0] > 0.5)); - response.traversability.push_back(result[1]); - response.untraversable_polygons.push_back(untraversable_polygonstamped); - } - return true; -} - -bool ElevationMappingNode::setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { - enablePointCloudPublishing_ = request.data; - response.success = true; - return true; -} - -void ElevationMappingNode::updateVariance(const ros::TimerEvent&) { - map_.update_variance(); -} - -void ElevationMappingNode::updateTime(const ros::TimerEvent&) { - map_.update_time(); -} - -void ElevationMappingNode::publishStatistics(const ros::TimerEvent&) { - ros::Time now = ros::Time::now(); - double dt = (now - lastStatisticsPublishedTime_).toSec(); - lastStatisticsPublishedTime_ = now; - elevation_map_msgs::Statistics msg; - msg.header.stamp = now; - if (dt > 0.0) { - msg.pointcloud_process_fps = pointCloudProcessCounter_ / dt; - } - pointCloudProcessCounter_ = 0; - statisticsPub_.publish(msg); -} - -void ElevationMappingNode::updateGridMap(const ros::TimerEvent&) { - std::vector layers(map_layers_all_.begin(), map_layers_all_.end()); - std::lock_guard lock(mapMutex_); - map_.get_grid_map(gridMap_, layers); - gridMap_.setTimestamp(ros::Time::now().toNSec()); - alivePub_.publish(std_msgs::Empty()); - - // Mostly debug purpose - if (enablePointCloudPublishing_) { - publishAsPointCloud(gridMap_); - } - if (enableNormalArrowPublishing_) { - publishNormalAsArrow(gridMap_); - } - isGridmapUpdated_ = true; -} - -bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request& request, - elevation_map_msgs::Initialize::Response& response) { - // If initialize method is points - if (request.type == request.POINTS) { - std::vector points; - for (const auto& point : request.points) { - const auto& pointFrameId = point.header.frame_id; - const auto& timeStamp = point.header.stamp; - const auto& pvector = Eigen::Vector3d(point.point.x, point.point.y, point.point.z); - - // Get tf from map frame to points' frame - if (mapFrameId_ != pointFrameId) { - Eigen::Affine3d transformationBaseToMap; - tf::StampedTransform transformTf; - try { - transformListener_.waitForTransform(mapFrameId_, pointFrameId, timeStamp, ros::Duration(1.0)); - transformListener_.lookupTransform(mapFrameId_, pointFrameId, timeStamp, transformTf); - poseTFToEigen(transformTf, transformationBaseToMap); - } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); - return false; - } - const auto transformed_p = transformationBaseToMap * pvector; - points.push_back(transformed_p); - } else { - points.push_back(pvector); - } - } - std::string method; - switch (request.method) { - case request.NEAREST: - method = "nearest"; - break; - case request.LINEAR: - method = "linear"; - break; - case request.CUBIC: - method = "cubic"; - break; - } - ROS_INFO_STREAM("Initializing map with points using " << method); - map_.initializeWithPoints(points, method); - } - response.success = true; - return true; -} - -void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const { - auto startTime = ros::Time::now(); - - const auto& normalX = map["normal_x"]; - const auto& normalY = map["normal_y"]; - const auto& normalZ = map["normal_z"]; - double scale = 0.1; - - visualization_msgs::MarkerArray markerArray; - // For each cell in map. - for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { - if (!map.isValid(*iterator, "elevation")) { - continue; - } - grid_map::Position3 p; - map.getPosition3("elevation", *iterator, p); - Eigen::Vector3d start = p; - const auto i = iterator.getLinearIndex(); - Eigen::Vector3d normal(normalX(i), normalY(i), normalZ(i)); - Eigen::Vector3d end = start + normal * scale; - if (normal.norm() < 0.1) { - continue; - } - markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); - } - normalPub_.publish(markerArray); - ROS_INFO_THROTTLE(1.0, "publish as normal in %f sec.", (ros::Time::now() - startTime).toSec()); -} - -visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, - const int id) const { - visualization_msgs::Marker marker; - marker.header.frame_id = mapFrameId_; - marker.header.stamp = ros::Time::now(); - marker.ns = "normal"; - marker.id = id; - marker.type = visualization_msgs::Marker::ARROW; - marker.action = visualization_msgs::Marker::ADD; - marker.points.resize(2); - marker.points[0].x = start.x(); - marker.points[0].y = start.y(); - marker.points[0].z = start.z(); - marker.points[1].x = end.x(); - marker.points[1].y = end.y(); - marker.points[1].z = end.z(); - marker.pose.orientation.x = 0.0; - marker.pose.orientation.y = 0.0; - marker.pose.orientation.z = 0.0; - marker.pose.orientation.w = 1.0; - marker.scale.x = 0.01; - marker.scale.y = 0.01; - marker.scale.z = 0.01; - marker.color.a = 1.0; // Don't forget to set the alpha! - marker.color.r = 0.0; - marker.color.g = 1.0; - marker.color.b = 0.0; - - return marker; -} - -void ElevationMappingNode::publishMapToOdom(double error) { - tf::Transform transform; - transform.setOrigin(tf::Vector3(0.0, 0.0, error)); - tf::Quaternion q; - q.setRPY(0, 0, 0); - transform.setRotation(q); - tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_)); -} - -} // namespace elevation_mapping_cupy +// // Get pose of sensor in map frame +// tf::StampedTransform transformTf; +// std::string sensorFrameId = image_msg->header.frame_id; +// auto timeStamp = image_msg->header.stamp; +// Eigen::Affine3d transformationMapToSensor; +// try { +// transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(sensorFrameId, mapFrameId_, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationMapToSensor); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return; +// } + +// // Transform image to vector of Eigen matrices for easy pybind conversion +// std::vector image_split; +// std::vector multichannel_image; +// cv::split(image, image_split); +// for (auto img : image_split) { +// ColMatrixXf eigen_img; +// cv::cv2eigen(img, eigen_img); +// multichannel_image.push_back(eigen_img); +// } + +// // Check if the size of multichannel_image and channels and channel_methods matches. "rgb" counts for 3 layers. +// int total_channels = 0; +// for (const auto& channel : channels) { +// if (channel == "rgb") { +// total_channels += 3; +// } else { +// total_channels += 1; +// } +// } +// if (total_channels != multichannel_image.size()) { +// ROS_ERROR("Mismatch in the size of multichannel_image (%d), channels (%d). Please check the input.", multichannel_image.size(), channels.size()); +// ROS_ERROR_STREAM("Current Channels: " << boost::algorithm::join(channels, ", ")); +// return; +// } + +// // Pass image to pipeline +// map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, +// distortionCoeffs, distortion_model, image.rows, image.cols); +// } + +// void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, +// const sensor_msgs::CameraInfoConstPtr& camera_info_msg, +// const std::string& key) { +// auto start = ros::Time::now(); +// inputImage(image_msg, camera_info_msg, channels_[key]); +// ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); +// } + +// void ElevationMappingNode::imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, +// const sensor_msgs::CameraInfoConstPtr& camera_info_msg, +// const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg) { +// auto start = ros::Time::now(); +// // Default channels and fusion methods for image is rgb and image_color +// std::vector channels; +// channels = channel_info_msg->channels; +// inputImage(image_msg, camera_info_msg, channels); +// ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); +// } + +// void ElevationMappingNode::updatePose(const ros::TimerEvent&) { +// tf::StampedTransform transformTf; +// const auto& timeStamp = ros::Time::now(); +// Eigen::Affine3d transformationBaseToMap; +// try { +// transformListener_.waitForTransform(mapFrameId_, baseFrameId_, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(mapFrameId_, baseFrameId_, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationBaseToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return; +// } + +// // This is to check if the robot is moving. If the robot is not moving, drift compensation is disabled to avoid creating artifacts. +// Eigen::Vector3d position(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); +// map_.move_to(position, transformationBaseToMap.rotation().transpose()); +// Eigen::Vector3d position3(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); +// Eigen::Vector4d orientation(transformTf.getRotation().x(), transformTf.getRotation().y(), transformTf.getRotation().z(), +// transformTf.getRotation().w()); +// lowpassPosition_ = positionAlpha_ * position3 + (1 - positionAlpha_) * lowpassPosition_; +// lowpassOrientation_ = orientationAlpha_ * orientation + (1 - orientationAlpha_) * lowpassOrientation_; +// { +// std::lock_guard lock(errorMutex_); +// positionError_ = (position3 - lowpassPosition_).norm(); +// orientationError_ = (orientation - lowpassOrientation_).norm(); +// } + +// if (useInitializerAtStart_) { +// ROS_INFO("Clearing map with initializer."); +// initializeWithTF(); +// useInitializerAtStart_ = false; +// } +// } + +// void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const { +// sensor_msgs::PointCloud2 msg; +// grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg); +// pointPub_.publish(msg); +// } + +// bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) { +// std::string requestedFrameId = request.frame_id; +// Eigen::Isometry3d transformationOdomToMap; +// grid_map::Position requestedSubmapPosition(request.position_x, request.position_y); +// if (requestedFrameId != mapFrameId_) { +// tf::StampedTransform transformTf; +// const auto& timeStamp = ros::Time::now(); +// try { +// transformListener_.waitForTransform(requestedFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(requestedFrameId, mapFrameId_, timeStamp, transformTf); +// tf::poseTFToEigen(transformTf, transformationOdomToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return false; +// } +// Eigen::Vector3d p(request.position_x, request.position_y, 0); +// Eigen::Vector3d mapP = transformationOdomToMap.inverse() * p; +// requestedSubmapPosition.x() = mapP.x(); +// requestedSubmapPosition.y() = mapP.y(); +// } +// grid_map::Length requestedSubmapLength(request.length_x, request.length_y); +// ROS_DEBUG("Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", requestedSubmapPosition.x(), requestedSubmapPosition.y(), +// requestedSubmapLength(0), requestedSubmapLength(1)); + +// bool isSuccess; +// grid_map::Index index; +// grid_map::GridMap subMap; +// { +// std::lock_guard lock(mapMutex_); +// subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, index, isSuccess); +// } +// const auto& length = subMap.getLength(); +// if (requestedFrameId != mapFrameId_) { +// subMap = subMap.getTransformedMap(transformationOdomToMap, "elevation", requestedFrameId); +// } + +// if (request.layers.empty()) { +// grid_map::GridMapRosConverter::toMessage(subMap, response.map); +// } else { +// std::vector layers; +// for (const auto& layer : request.layers) { +// layers.push_back(layer); +// } +// grid_map::GridMapRosConverter::toMessage(subMap, layers, response.map); +// } +// return isSuccess; +// } + +// bool ElevationMappingNode::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { +// ROS_INFO("Clearing map."); +// map_.clear(); +// if (alwaysClearWithInitializer_) { +// initializeWithTF(); +// } +// return true; +// } + +// bool ElevationMappingNode::clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { +// ROS_INFO("Clearing map with initializer."); +// map_.clear(); +// initializeWithTF(); +// return true; +// } + +// void ElevationMappingNode::initializeWithTF() { +// std::vector points; +// const auto& timeStamp = ros::Time::now(); +// int i = 0; +// Eigen::Vector3d p; +// for (const auto& frame_id : initialize_frame_id_) { +// // Get tf from map frame to tf frame +// Eigen::Affine3d transformationBaseToMap; +// tf::StampedTransform transformTf; +// try { +// transformListener_.waitForTransform(mapFrameId_, frame_id, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(mapFrameId_, frame_id, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationBaseToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return; +// } +// p = transformationBaseToMap.translation(); +// p.z() += initialize_tf_offset_[i]; +// points.push_back(p); +// i++; +// } +// if (!points.empty() && points.size() < 3) { +// points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, initializeTfGridSize_, 0)); +// points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, initializeTfGridSize_, 0)); +// points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, -initializeTfGridSize_, 0)); +// points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, -initializeTfGridSize_, 0)); +// } +// ROS_INFO_STREAM("Initializing map with points using " << initializeMethod_); +// map_.initializeWithPoints(points, initializeMethod_); +// } + +// bool ElevationMappingNode::checkSafety(elevation_map_msgs::CheckSafety::Request& request, +// elevation_map_msgs::CheckSafety::Response& response) { +// for (const auto& polygonstamped : request.polygons) { +// if (polygonstamped.polygon.points.empty()) { +// continue; +// } +// std::vector polygon; +// std::vector untraversable_polygon; +// Eigen::Vector3d result; +// result.setZero(); +// const auto& polygonFrameId = polygonstamped.header.frame_id; +// const auto& timeStamp = polygonstamped.header.stamp; +// double polygon_z = polygonstamped.polygon.points[0].z; + +// // Get tf from map frame to polygon frame +// if (mapFrameId_ != polygonFrameId) { +// Eigen::Affine3d transformationBaseToMap; +// tf::StampedTransform transformTf; +// try { +// transformListener_.waitForTransform(mapFrameId_, polygonFrameId, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(mapFrameId_, polygonFrameId, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationBaseToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return false; +// } +// for (const auto& p : polygonstamped.polygon.points) { +// const auto& pvector = Eigen::Vector3d(p.x, p.y, p.z); +// const auto transformed_p = transformationBaseToMap * pvector; +// polygon.emplace_back(Eigen::Vector2d(transformed_p.x(), transformed_p.y())); +// } +// } else { +// for (const auto& p : polygonstamped.polygon.points) { +// polygon.emplace_back(Eigen::Vector2d(p.x, p.y)); +// } +// } + +// map_.get_polygon_traversability(polygon, result, untraversable_polygon); + +// geometry_msgs::PolygonStamped untraversable_polygonstamped; +// untraversable_polygonstamped.header.stamp = ros::Time::now(); +// untraversable_polygonstamped.header.frame_id = mapFrameId_; +// for (const auto& p : untraversable_polygon) { +// geometry_msgs::Point32 point; +// point.x = static_cast(p.x()); +// point.y = static_cast(p.y()); +// point.z = static_cast(polygon_z); +// untraversable_polygonstamped.polygon.points.push_back(point); +// } +// // traversability_result; +// response.is_safe.push_back(bool(result[0] > 0.5)); +// response.traversability.push_back(result[1]); +// response.untraversable_polygons.push_back(untraversable_polygonstamped); +// } +// return true; +// } + +// bool ElevationMappingNode::setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { +// enablePointCloudPublishing_ = request.data; +// response.success = true; +// return true; +// } + +// void ElevationMappingNode::updateVariance(const ros::TimerEvent&) { +// map_.update_variance(); +// } + +// void ElevationMappingNode::updateTime(const ros::TimerEvent&) { +// map_.update_time(); +// } + +// void ElevationMappingNode::publishStatistics(const ros::TimerEvent&) { +// ros::Time now = ros::Time::now(); +// double dt = (now - lastStatisticsPublishedTime_).toSec(); +// lastStatisticsPublishedTime_ = now; +// elevation_map_msgs::Statistics msg; +// msg.header.stamp = now; +// if (dt > 0.0) { +// msg.pointcloud_process_fps = pointCloudProcessCounter_ / dt; +// } +// pointCloudProcessCounter_ = 0; +// statisticsPub_.publish(msg); +// } + +// void ElevationMappingNode::updateGridMap(const ros::TimerEvent&) { +// std::vector layers(map_layers_all_.begin(), map_layers_all_.end()); +// std::lock_guard lock(mapMutex_); +// map_.get_grid_map(gridMap_, layers); +// gridMap_.setTimestamp(ros::Time::now().toNSec()); +// alivePub_.publish(std_msgs::Empty()); + +// // Mostly debug purpose +// if (enablePointCloudPublishing_) { +// publishAsPointCloud(gridMap_); +// } +// if (enableNormalArrowPublishing_) { +// publishNormalAsArrow(gridMap_); +// } +// isGridmapUpdated_ = true; +// } + +// bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request& request, +// elevation_map_msgs::Initialize::Response& response) { +// // If initialize method is points +// if (request.type == request.POINTS) { +// std::vector points; +// for (const auto& point : request.points) { +// const auto& pointFrameId = point.header.frame_id; +// const auto& timeStamp = point.header.stamp; +// const auto& pvector = Eigen::Vector3d(point.point.x, point.point.y, point.point.z); + +// // Get tf from map frame to points' frame +// if (mapFrameId_ != pointFrameId) { +// Eigen::Affine3d transformationBaseToMap; +// tf::StampedTransform transformTf; +// try { +// transformListener_.waitForTransform(mapFrameId_, pointFrameId, timeStamp, ros::Duration(1.0)); +// transformListener_.lookupTransform(mapFrameId_, pointFrameId, timeStamp, transformTf); +// poseTFToEigen(transformTf, transformationBaseToMap); +// } catch (tf::TransformException& ex) { +// ROS_ERROR("%s", ex.what()); +// return false; +// } +// const auto transformed_p = transformationBaseToMap * pvector; +// points.push_back(transformed_p); +// } else { +// points.push_back(pvector); +// } +// } +// std::string method; +// switch (request.method) { +// case request.NEAREST: +// method = "nearest"; +// break; +// case request.LINEAR: +// method = "linear"; +// break; +// case request.CUBIC: +// method = "cubic"; +// break; +// } +// ROS_INFO_STREAM("Initializing map with points using " << method); +// map_.initializeWithPoints(points, method); +// } +// response.success = true; +// return true; +// } + +// void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const { +// auto startTime = ros::Time::now(); + +// const auto& normalX = map["normal_x"]; +// const auto& normalY = map["normal_y"]; +// const auto& normalZ = map["normal_z"]; +// double scale = 0.1; + +// visualization_msgs::MarkerArray markerArray; +// // For each cell in map. +// for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { +// if (!map.isValid(*iterator, "elevation")) { +// continue; +// } +// grid_map::Position3 p; +// map.getPosition3("elevation", *iterator, p); +// Eigen::Vector3d start = p; +// const auto i = iterator.getLinearIndex(); +// Eigen::Vector3d normal(normalX(i), normalY(i), normalZ(i)); +// Eigen::Vector3d end = start + normal * scale; +// if (normal.norm() < 0.1) { +// continue; +// } +// markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); +// } +// normalPub_.publish(markerArray); +// ROS_INFO_THROTTLE(1.0, "publish as normal in %f sec.", (ros::Time::now() - startTime).toSec()); +// } + +// visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, +// const int id) const { +// visualization_msgs::Marker marker; +// marker.header.frame_id = mapFrameId_; +// marker.header.stamp = ros::Time::now(); +// marker.ns = "normal"; +// marker.id = id; +// marker.type = visualization_msgs::Marker::ARROW; +// marker.action = visualization_msgs::Marker::ADD; +// marker.points.resize(2); +// marker.points[0].x = start.x(); +// marker.points[0].y = start.y(); +// marker.points[0].z = start.z(); +// marker.points[1].x = end.x(); +// marker.points[1].y = end.y(); +// marker.points[1].z = end.z(); +// marker.pose.orientation.x = 0.0; +// marker.pose.orientation.y = 0.0; +// marker.pose.orientation.z = 0.0; +// marker.pose.orientation.w = 1.0; +// marker.scale.x = 0.01; +// marker.scale.y = 0.01; +// marker.scale.z = 0.01; +// marker.color.a = 1.0; // Don't forget to set the alpha! +// marker.color.r = 0.0; +// marker.color.g = 1.0; +// marker.color.b = 0.0; + +// return marker; +// } + +// void ElevationMappingNode::publishMapToOdom(double error) { +// tf::Transform transform; +// transform.setOrigin(tf::Vector3(0.0, 0.0, error)); +// tf::Quaternion q; +// q.setRPY(0, 0, 0); +// transform.setRotation(q); +// tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_)); +// } + +// } // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index 3980a8be..16c57332 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -12,162 +12,171 @@ #include // ROS -#include +#include +#include #include + namespace elevation_mapping_cupy { ElevationMappingWrapper::ElevationMappingWrapper() {} -void ElevationMappingWrapper::initialize(ros::NodeHandle& nh) { +void ElevationMappingWrapper::initialize(rclcpp::Node::SharedPtr node) { // Add the elevation_mapping_cupy path to sys.path auto threading = py::module::import("threading"); py::gil_scoped_acquire acquire; auto sys = py::module::import("sys"); auto path = sys.attr("path"); - std::string module_path = ros::package::getPath("elevation_mapping_cupy"); + std::string module_path = ament_index_cpp::get_package_share_directory("elevation_mapping_cupy"); module_path = module_path + "/script"; path.attr("insert")(0, module_path); auto elevation_mapping = py::module::import("elevation_mapping_cupy.elevation_mapping"); auto parameter = py::module::import("elevation_mapping_cupy.parameter"); param_ = parameter.attr("Parameter")(); - setParameters(nh); + setParameters(node); map_ = elevation_mapping.attr("ElevationMap")(param_); } -/** - * Load ros parameters into Parameter class. - * Search for the same name within the name space. - */ -void ElevationMappingWrapper::setParameters(ros::NodeHandle& nh) { +// /** +// * Load ros parameters into Parameter class. +// * Search for the same name within the name space. +// */ +void ElevationMappingWrapper::setParameters(rclcpp::Node::SharedPtr node) { // Get all parameters names and types. py::list paramNames = param_.attr("get_names")(); py::list paramTypes = param_.attr("get_types")(); py::gil_scoped_acquire acquire; - // Try to find the parameter in the ros parameter server. + // Try to find the parameter in the ROS parameter server. // If there was a parameter, set it to the Parameter variable. - for (int i = 0; i < paramNames.size(); i++) { + for (size_t i = 0; i < paramNames.size(); i++) { std::string type = py::cast(paramTypes[i]); std::string name = py::cast(paramNames[i]); if (type == "float") { float param; - if (nh.getParam(name, param)) { + if (node->get_parameter(name, param)) { param_.attr("set_value")(name, param); } } else if (type == "str") { std::string param; - if (nh.getParam(name, param)) { + if (node->get_parameter(name, param)) { param_.attr("set_value")(name, param); } } else if (type == "bool") { bool param; - if (nh.getParam(name, param)) { + if (node->get_parameter(name, param)) { param_.attr("set_value")(name, param); } } else if (type == "int") { int param; - if (nh.getParam(name, param)) { + if (node->get_parameter(name, param)) { param_.attr("set_value")(name, param); } } } - XmlRpc::XmlRpcValue subscribers; - nh.getParam("subscribers", subscribers); - - py::dict sub_dict; - for (auto& subscriber : subscribers) { - const char* const name = subscriber.first.c_str(); - const auto& subscriber_params = subscriber.second; - if (!sub_dict.contains(name)) { - sub_dict[name] = py::dict(); - } - for (auto iterat : subscriber_params) { - const char* const key = iterat.first.c_str(); - const auto val = iterat.second; - std::vector arr; - switch (val.getType()) { - case XmlRpc::XmlRpcValue::TypeString: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeInt: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeDouble: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeBoolean: - sub_dict[name][key] = static_cast(val); - break; - case XmlRpc::XmlRpcValue::TypeArray: - for (int32_t i = 0; i < val.size(); ++i) { - auto elem = static_cast(val[i]); - arr.push_back(elem); + + rclcpp::Parameter subscribers; + node->get_parameter("subscribers", subscribers); + + py::dict sub_dict; + auto subscribers_array = subscribers.as_string_array(); + for (const auto& subscriber : subscribers_array) { + const char* const name = subscriber.c_str(); + if (!sub_dict.contains(name)) { + sub_dict[name] = py::dict(); + } + std::map subscriber_params; + node->get_parameters(name, subscriber_params); + for (const auto& param_pair : subscriber_params) { + const char* const param_name = param_pair.first.c_str(); + const auto& param_value = param_pair.second; + std::vector arr; + switch (param_value.get_type()) { + case rclcpp::ParameterType::PARAMETER_STRING: + sub_dict[name][param_name] = param_value.as_string(); + break; + case rclcpp::ParameterType::PARAMETER_INTEGER: + sub_dict[name][param_name] = param_value.as_int(); + break; + case rclcpp::ParameterType::PARAMETER_DOUBLE: + sub_dict[name][param_name] = param_value.as_double(); + break; + case rclcpp::ParameterType::PARAMETER_BOOL: + sub_dict[name][param_name] = param_value.as_bool(); + break; + case rclcpp::ParameterType::PARAMETER_STRING_ARRAY: + for (const auto& elem : param_value.as_string_array()) { + arr.push_back(elem); + } + sub_dict[name][param_name] = arr; + arr.clear(); + break; + default: + sub_dict[name][param_name] = py::cast(param_value); + break; } - sub_dict[name][key] = arr; - arr.clear(); - break; - case XmlRpc::XmlRpcValue::TypeStruct: - break; - default: - sub_dict[name][key] = py::cast(val); - break; + } } - } - } - param_.attr("subscriber_cfg") = sub_dict; - - // point cloud channel fusion - if (!nh.hasParam("pointcloud_channel_fusions")) { - ROS_WARN("No pointcloud_channel_fusions parameter found. Using default values."); - } - else { - XmlRpc::XmlRpcValue pointcloud_channel_fusion; - nh.getParam("pointcloud_channel_fusions", pointcloud_channel_fusion); - - py::dict pointcloud_channel_fusion_dict; - for (auto& channel_fusion : pointcloud_channel_fusion) { - const char* const name = channel_fusion.first.c_str(); - std::string fusion = static_cast(channel_fusion.second); - if (!pointcloud_channel_fusion_dict.contains(name)) { - pointcloud_channel_fusion_dict[name] = fusion; + param_.attr("subscriber_cfg") = sub_dict; + + + // point cloud channel fusion + if (!node->has_parameter("pointcloud_channel_fusions")) { + RCLCPP_WARN(node->get_logger(), "No pointcloud_channel_fusions parameter found. Using default values."); + } else { + rclcpp::Parameter pointcloud_channel_fusion; + node->get_parameter("pointcloud_channel_fusions", pointcloud_channel_fusion); + + py::dict pointcloud_channel_fusion_dict; + auto pointcloud_channel_fusion_map = pointcloud_channel_fusion.as_string_array(); + for (const auto& channel_fusion : pointcloud_channel_fusion_map) { + const char* const fusion_name = channel_fusion.c_str(); + std::string fusion; + node->get_parameter(fusion_name, fusion); + if (!pointcloud_channel_fusion_dict.contains(fusion_name)) { + pointcloud_channel_fusion_dict[fusion_name] = fusion; + } + } + RCLCPP_INFO_STREAM(node->get_logger(), "pointcloud_channel_fusion_dict: " << pointcloud_channel_fusion_dict); + param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict; } - } - ROS_INFO_STREAM("pointcloud_channel_fusion_dict: " << pointcloud_channel_fusion_dict); - param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict; - } - // image channel fusion - if (!nh.hasParam("image_channel_fusions")) { - ROS_WARN("No image_channel_fusions parameter found. Using default values."); - } - else { - XmlRpc::XmlRpcValue image_channel_fusion; - nh.getParam("image_channel_fusions", image_channel_fusion); - - py::dict image_channel_fusion_dict; - for (auto& channel_fusion : image_channel_fusion) { - const char* const name = channel_fusion.first.c_str(); - std::string fusion = static_cast(channel_fusion.second); - if (!image_channel_fusion_dict.contains(name)) { - image_channel_fusion_dict[name] = fusion; + + // image channel fusion + if (!node->has_parameter("image_channel_fusions")) { + RCLCPP_WARN(node->get_logger(), "No image_channel_fusions parameter found. Using default values."); + } else { + rclcpp::Parameter image_channel_fusion; + node->get_parameter("image_channel_fusions", image_channel_fusion); + + py::dict image_channel_fusion_dict; + auto image_channel_fusion_map = image_channel_fusion.as_string_array(); + for (const auto& channel_fusion : image_channel_fusion_map) { + const char* const channel_fusion_name = channel_fusion.c_str(); + std::string fusion; + node->get_parameter(channel_fusion_name, fusion); + if (!image_channel_fusion_dict.contains(channel_fusion_name)) { + image_channel_fusion_dict[channel_fusion_name] = fusion; + } + } + RCLCPP_INFO_STREAM(node->get_logger(), "image_channel_fusion_dict: " << image_channel_fusion_dict); + param_.attr("image_channel_fusions") = image_channel_fusion_dict; } - } - ROS_INFO_STREAM("image_channel_fusion_dict: " << image_channel_fusion_dict); - param_.attr("image_channel_fusions") = image_channel_fusion_dict; - } - param_.attr("update")(); - resolution_ = py::cast(param_.attr("get_value")("resolution")); - map_length_ = py::cast(param_.attr("get_value")("true_map_length")); - map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); + param_.attr("update")(); + resolution_ = py::cast(param_.attr("get_value")("resolution")); + map_length_ = py::cast(param_.attr("get_value")("true_map_length")); + map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); + + node->declare_parameter("enable_normal", false); + node->declare_parameter("enable_normal_color", false); + enable_normal_ = node->get_parameter("enable_normal").as_bool(); + enable_normal_color_ = node->get_parameter("enable_normal_color").as_bool(); - nh.param("enable_normal", enable_normal_, false); - nh.param("enable_normal_color", enable_normal_color_, false); } void ElevationMappingWrapper::input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, From 1dd0e86f248cb2ca03fac4be072fe10158585ab6 Mon Sep 17 00:00:00 2001 From: amilearning Date: Tue, 5 Nov 2024 00:30:09 +0100 Subject: [PATCH 02/14] ros2 --- elevation_mapping_cupy/CMakeLists.txt | 53 +- .../lib/elevation_mapping_cupy/__init__.py | 4 + .../elevation_mapping.py | 969 +++++++++++++ .../elevation_mapping_ros.py | 322 ++++ .../elevation_mapping_cupy/fusion/__init__.py | 0 .../fusion/fusion_manager.py | 112 ++ .../fusion/image_color.py | 86 ++ .../fusion/image_exponential.py | 77 + .../fusion/pointcloud_average.py | 113 ++ .../fusion/pointcloud_bayesian_inference.py | 122 ++ .../fusion/pointcloud_class_average.py | 126 ++ .../fusion/pointcloud_class_bayesian.py | 75 + .../fusion/pointcloud_class_max.py | 123 ++ .../fusion/pointcloud_color.py | 152 ++ .../kernels/__init__.py | 3 + .../kernels/custom_image_kernels.py | 271 ++++ .../kernels/custom_kernels.py | 685 +++++++++ .../kernels/custom_semantic_kernels.py | 375 +++++ .../lib/elevation_mapping_cupy/kernels/kk.py | 1290 +++++++++++++++++ .../elevation_mapping_cupy/map_initializer.py | 80 + .../lib/elevation_mapping_cupy/parameter.py | 300 ++++ .../plugins/__init__.py | 0 .../elevation_mapping_cupy/plugins/erosion.py | 113 ++ .../plugins/features_pca.py | 96 ++ .../plugins/inpainting.py | 63 + .../plugins/max_layer_filter.py | 108 ++ .../plugins/min_filter.py | 118 ++ .../plugins/plugin_manager.py | 268 ++++ .../plugins/robot_centric_elevation.py | 121 ++ .../plugins/semantic_filter.py | 133 ++ .../plugins/semantic_traversability.py | 83 ++ .../plugins/smooth_filter.py | 59 + .../elevation_mapping_cupy/semantic_map.py | 400 +++++ .../elevation_mapping_cupy/tests/__init__.py | 0 .../tests/test_elevation_mapping.py | 118 ++ .../tests/test_parameter.py | 17 + .../tests/test_plugins.py | 69 + .../tests/test_semantic_kernels.py | 307 ++++ .../tests/test_semantic_map.py | 47 + .../traversability_filter.py | 100 ++ .../traversability_polygon.py | 77 + .../config/core/core_param.yaml | 248 +++- .../config/core/example_setup.yaml | 159 +- .../elevation_mapping_ros.hpp | 242 ++-- .../elevation_mapping_wrapper.hpp | 12 +- .../launch/elevation_mapping_cupy.launch | 7 - .../launch/elevation_mapping_cupy.launch.py | 28 + elevation_mapping_cupy/package.xml | 2 + .../script/elevation_mapping_cupy/__init__.py | 6 +- .../elevation_mapping.py | 8 +- .../elevation_mapping_ros.py | 607 ++++---- .../elevation_mapping_cupy/kernels/kk.py | 1290 +++++++++++++++++ .../elevation_mapping_cupy/parameter.py | 16 +- .../traversability_polygon.py | 2 +- elevation_mapping_cupy/setup.py | 35 +- .../src/elevation_mapping_node.cpp | 15 +- .../src/elevation_mapping_ros.cpp | 519 ++++--- .../src/elevation_mapping_wrapper.cpp | 273 ++-- 58 files changed, 10154 insertions(+), 950 deletions(-) create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/__init__.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping_ros.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/__init__.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/fusion_manager.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_color.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_exponential.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_average.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_bayesian_inference.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_average.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_bayesian.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_max.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_color.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/__init__.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_image_kernels.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_kernels.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_semantic_kernels.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/kk.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/map_initializer.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/__init__.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/erosion.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/features_pca.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/inpainting.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/max_layer_filter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/min_filter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/plugin_manager.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/robot_centric_elevation.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_filter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_traversability.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/smooth_filter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/semantic_map.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/__init__.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_elevation_mapping.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_parameter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_plugins.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_kernels.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_map.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_filter.py create mode 100644 elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_polygon.py delete mode 100644 elevation_mapping_cupy/launch/elevation_mapping_cupy.launch create mode 100644 elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py create mode 100644 elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/kk.py diff --git a/elevation_mapping_cupy/CMakeLists.txt b/elevation_mapping_cupy/CMakeLists.txt index 81baa06d..fa8e2d25 100644 --- a/elevation_mapping_cupy/CMakeLists.txt +++ b/elevation_mapping_cupy/CMakeLists.txt @@ -1,16 +1,16 @@ cmake_minimum_required(VERSION 3.8) project(elevation_mapping_cupy) -# Enable C++11 (or higher if needed for ROS 2 and pybind11) -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +# # Enable C++11 (or higher if needed for ROS 2 and pybind11) +# set(CMAKE_CXX_STANDARD 11) +# set(CMAKE_CXX_STANDARD_REQUIRED ON) # Compiler options if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() # Additional dependencies -find_package(Python COMPONENTS Interpreter Development) +# find_package(Python COMPONENTS Interpreter Development) find_package(PythonInterp 3 REQUIRED) find_package(pybind11 CONFIG REQUIRED) find_package(Eigen3 REQUIRED) @@ -37,7 +37,9 @@ find_package(elevation_map_msgs REQUIRED) find_package(grid_map_ros REQUIRED) find_package(image_transport REQUIRED) find_package(pcl_ros REQUIRED) - +find_package(tf2_eigen REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(python_cmake_module REQUIRED) # List dependencies for ament_target_dependencies set(dependencies @@ -54,6 +56,7 @@ set(dependencies image_transport pcl_ros message_filters + tf2_eigen ) # Include directories @@ -72,29 +75,63 @@ add_library(elevation_mapping_ros ) # Link the library with necessary dependencies -target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES}) +target_link_libraries(elevation_mapping_ros ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES} pybind11::embed) ament_target_dependencies(elevation_mapping_ros ${dependencies}) # Declare C++ executable add_executable(elevation_mapping_node src/elevation_mapping_node.cpp) # Link the executable with the library and dependencies -target_link_libraries(elevation_mapping_node elevation_mapping_ros ${OpenCV_LIBRARIES}) +target_link_libraries(elevation_mapping_node elevation_mapping_ros ${OpenCV_LIBRARIES} pybind11::embed) ament_target_dependencies(elevation_mapping_node ${dependencies}) + + # Install targets install( - TARGETS elevation_mapping_ros elevation_mapping_node + TARGETS elevation_mapping_node + DESTINATION lib/${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) + +install(TARGETS + elevation_mapping_node + DESTINATION lib/${PROJECT_NAME} +) + + +install( + TARGETS elevation_mapping_ros + DESTINATION lib/${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + + +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR script/${PROJECT_NAME}) + +install(PROGRAMS + DESTINATION lib/${PROJECT_NAME} +) + # Install launch and config directories install( DIRECTORY launch config DESTINATION share/${PROJECT_NAME} ) + + +_ament_cmake_python_register_environment_hook() + + +# ament_python_install_package(script/${PROJECT_NAME}) +# ament_python_install_package(script/${PROJECT_NAME}) + + # Ament package declaration ament_package() diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/__init__.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/__init__.py new file mode 100644 index 00000000..88d6a666 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/__init__.py @@ -0,0 +1,4 @@ +# from .parameter import Parameter +# from .elevation_mapping import ElevationMap +# from .kernels import custom_image_kernels, custom_kernels, custom_semantic_kernels + diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py new file mode 100644 index 00000000..4d704739 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py @@ -0,0 +1,969 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import os +from typing import List, Any, Tuple, Union + +import numpy as np +import threading +import subprocess + +from elevation_mapping_cupy.traversability_filter import ( + get_filter_chainer, + get_filter_torch, +) +from elevation_mapping_cupy.parameter import Parameter + + +from elevation_mapping_cupy.kernels import ( + add_points_kernel, + add_color_kernel, + color_average_kernel, +) +from elevation_mapping_cupy.kernels import sum_kernel +from elevation_mapping_cupy.kernels import error_counting_kernel +from elevation_mapping_cupy.kernels import average_map_kernel +from elevation_mapping_cupy.kernels import dilation_filter_kernel +from elevation_mapping_cupy.kernels import normal_filter_kernel +from elevation_mapping_cupy.kernels import polygon_mask_kernel +from elevation_mapping_cupy.kernels import image_to_map_correspondence_kernel + +from elevation_mapping_cupy.map_initializer import MapInitializer +from elevation_mapping_cupy.plugins.plugin_manager import PluginManager +from elevation_mapping_cupy.semantic_map import SemanticMap +from elevation_mapping_cupy.traversability_polygon import ( + get_masked_traversability, + is_traversable, + calculate_area, + transform_to_map_position, + transform_to_map_index, +) + +import cupy as cp + +xp = cp +pool = cp.cuda.MemoryPool(cp.cuda.malloc_managed) +cp.cuda.set_allocator(pool.malloc) + + +class ElevationMap: + """Core elevation mapping class.""" + + def __init__(self, param: Parameter): + """ + + Args: + param (elevation_mapping_cupy.parameter.Parameter): + """ + self.param = param + self.data_type = self.param.data_type + self.resolution = param.resolution + self.center = xp.array([0, 0, 0], dtype=self.data_type) + self.base_rotation = xp.eye(3, dtype=self.data_type) + self.map_length = param.map_length + self.cell_n = param.cell_n + + self.map_lock = threading.Lock() + self.semantic_map = SemanticMap(self.param) + self.elevation_map = xp.zeros((7, self.cell_n, self.cell_n), dtype=self.data_type) + self.layer_names = [ + "elevation", + "variance", + "is_valid", + "traversability", + "time", + "upper_bound", + "is_upper_bound", + ] + + # buffers + self.traversability_buffer = xp.full((self.cell_n, self.cell_n), xp.nan) + self.normal_map = xp.zeros((3, self.cell_n, self.cell_n), dtype=self.data_type) + # Initial variance + self.initial_variance = param.initial_variance + self.elevation_map[1] += self.initial_variance + self.elevation_map[3] += 1.0 + + # overlap clearance + cell_range = int(self.param.overlap_clear_range_xy / self.resolution) + cell_range = np.clip(cell_range, 0, self.cell_n) + self.cell_min = self.cell_n // 2 - cell_range // 2 + self.cell_max = self.cell_n // 2 + cell_range // 2 + + # Initial mean_error + self.mean_error = 0.0 + self.additive_mean_error = 0.0 + + self.compile_kernels() + + self.compile_image_kernels() + + self.semantic_map.initialize_fusion() + + weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') + param.load_weights(weight_file) + + if param.use_chainer: + self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) + else: + self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) + self.untraversable_polygon = xp.zeros((1, 2)) + + # Plugins + self.plugin_manager = PluginManager(cell_n=self.cell_n) + plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') + self.plugin_manager.load_plugin_settings(plugin_config_file) + + self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") + + def clear(self): + """Reset all the layers of the elevation & the semantic map.""" + with self.map_lock: + self.elevation_map *= 0.0 + # Initial variance + self.elevation_map[1] += self.initial_variance + self.semantic_map.clear() + + self.mean_error = 0.0 + self.additive_mean_error = 0.0 + + def get_position(self, position): + """Return the position of the map center. + + Args: + position (numpy.ndarray): + + """ + position[0][:] = xp.asnumpy(self.center) + + def move(self, delta_position): + """Shift the map along all three axes according to the input. + + Args: + delta_position (numpy.ndarray): + """ + # Shift map using delta position. + delta_position = xp.asarray(delta_position) + delta_pixel = xp.round(delta_position[:2] / self.resolution) + delta_position_xy = delta_pixel * self.resolution + self.center[:2] += xp.asarray(delta_position_xy) + self.center[2] += xp.asarray(delta_position[2]) + self.shift_map_xy(delta_pixel) + self.shift_map_z(-delta_position[2]) + + def move_to(self, position, R): + """Shift the map to an absolute position and update the rotation of the robot. + + Args: + position (numpy.ndarray): + R (cupy._core.core.ndarray): + """ + # Shift map to the center of robot. + self.base_rotation = xp.asarray(R, dtype=self.data_type) + position = xp.asarray(position) + delta = position - self.center + delta_pixel = xp.around(delta[:2] / self.resolution) + delta_xy = delta_pixel * self.resolution + self.center[:2] += delta_xy + self.center[2] += delta[2] + self.shift_map_xy(-delta_pixel) + self.shift_map_z(-delta[2]) + + def pad_value(self, x, shift_value, idx=None, value=0.0): + """Create a padding of the map along x,y-axis according to amount that has shifted. + + Args: + x (cupy._core.core.ndarray): + shift_value (cupy._core.core.ndarray): + idx (Union[None, int, None, None]): + value (float): + """ + if idx is None: + if shift_value[0] > 0: + x[:, : shift_value[0], :] = value + elif shift_value[0] < 0: + x[:, shift_value[0] :, :] = value + if shift_value[1] > 0: + x[:, :, : shift_value[1]] = value + elif shift_value[1] < 0: + x[:, :, shift_value[1] :] = value + else: + if shift_value[0] > 0: + x[idx, : shift_value[0], :] = value + elif shift_value[0] < 0: + x[idx, shift_value[0] :, :] = value + if shift_value[1] > 0: + x[idx, :, : shift_value[1]] = value + elif shift_value[1] < 0: + x[idx, :, shift_value[1] :] = value + + def shift_map_xy(self, delta_pixel): + """Shift the map along the horizontal axes according to the input. + + Args: + delta_pixel (cupy._core.core.ndarray): + + """ + shift_value = delta_pixel.astype(cp.int32) + if cp.abs(shift_value).sum() == 0: + return + with self.map_lock: + self.elevation_map = cp.roll(self.elevation_map, shift_value, axis=(1, 2)) + self.pad_value(self.elevation_map, shift_value, value=0.0) + self.pad_value(self.elevation_map, shift_value, idx=1, value=self.initial_variance) + self.semantic_map.shift_map_xy(shift_value) + + def shift_map_z(self, delta_z): + """Shift the relevant layers along the vertical axis. + + Args: + delta_z (cupy._core.core.ndarray): + """ + with self.map_lock: + # elevation + self.elevation_map[0] += delta_z + # upper bound + self.elevation_map[5] += delta_z + + def compile_kernels(self): + """Compile all kernels belonging to the elevation map.""" + + self.new_map = cp.zeros( + (self.elevation_map.shape[0], self.cell_n, self.cell_n), + dtype=self.data_type, + ) + self.traversability_input = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) + self.traversability_mask_dummy = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) + self.min_filtered = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) + self.min_filtered_mask = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) + self.mask = cp.zeros((self.cell_n, self.cell_n), dtype=self.data_type) + self.add_points_kernel = add_points_kernel( + self.resolution, + self.cell_n, + self.cell_n, + self.param.sensor_noise_factor, + self.param.mahalanobis_thresh, + self.param.outlier_variance, + self.param.wall_num_thresh, + self.param.max_ray_length, + self.param.cleanup_step, + self.param.min_valid_distance, + self.param.max_height_range, + self.param.cleanup_cos_thresh, + self.param.ramped_height_range_a, + self.param.ramped_height_range_b, + self.param.ramped_height_range_c, + self.param.enable_edge_sharpen, + self.param.enable_visibility_cleanup, + ) + self.error_counting_kernel = error_counting_kernel( + self.resolution, + self.cell_n, + self.cell_n, + self.param.sensor_noise_factor, + self.param.mahalanobis_thresh, + self.param.drift_compensation_variance_inlier, + self.param.traversability_inlier, + self.param.min_valid_distance, + self.param.max_height_range, + self.param.ramped_height_range_a, + self.param.ramped_height_range_b, + self.param.ramped_height_range_c, + ) + self.average_map_kernel = average_map_kernel( + self.cell_n, self.cell_n, self.param.max_variance, self.initial_variance + ) + + self.dilation_filter_kernel = dilation_filter_kernel(self.cell_n, self.cell_n, self.param.dilation_size) + self.dilation_filter_kernel_initializer = dilation_filter_kernel( + self.cell_n, self.cell_n, self.param.dilation_size_initialize + ) + self.polygon_mask_kernel = polygon_mask_kernel(self.cell_n, self.cell_n, self.resolution) + self.normal_filter_kernel = normal_filter_kernel(self.cell_n, self.cell_n, self.resolution) + + def compile_image_kernels(self): + """Compile kernels related to processing image messages.""" + + for config in self.param.subscriber_cfg.values(): + if config["data_type"] == "image": + self.valid_correspondence = cp.asarray( + np.zeros((self.cell_n, self.cell_n), dtype=np.bool_), dtype=np.bool_ + ) + self.uv_correspondence = cp.asarray( + np.zeros((2, self.cell_n, self.cell_n), dtype=np.float32), + dtype=np.float32, + ) + # self.distance_correspondence = cp.asarray( + # np.zeros((self.cell_n, self.cell_n), dtype=np.float32), dtype=np.float32 + # ) + # TODO tolerance_z_collision add parameter + self.image_to_map_correspondence_kernel = image_to_map_correspondence_kernel( + resolution=self.resolution, + width=self.cell_n, + height=self.cell_n, + tolerance_z_collision=0.10, + ) + break + + def shift_translation_to_map_center(self, t): + """Deduct the map center to get the translation of a point w.r.t. the map center. + + Args: + t (cupy._core.core.ndarray): Absolute point position + """ + t -= self.center + + def update_map_with_kernel(self, points_all, channels, R, t, position_noise, orientation_noise): + """Update map with new measurement. + + Args: + points_all (cupy._core.core.ndarray): + channels (List[str]): + R (cupy._core.core.ndarray): + t (cupy._core.core.ndarray): + position_noise (float): + orientation_noise (float): + """ + self.new_map *= 0.0 + error = cp.array([0.0], dtype=cp.float32) + error_cnt = cp.array([0], dtype=cp.float32) + points = points_all[:, :3] + # additional_fusion = self.get_fusion_of_pcl(channels) + with self.map_lock: + self.shift_translation_to_map_center(t) + self.error_counting_kernel( + self.elevation_map, + points, + cp.array([0.0], dtype=self.data_type), + cp.array([0.0], dtype=self.data_type), + R, + t, + self.new_map, + error, + error_cnt, + size=(points.shape[0]), + ) + if ( + self.param.enable_drift_compensation + and error_cnt > self.param.min_height_drift_cnt + and ( + position_noise > self.param.position_noise_thresh + or orientation_noise > self.param.orientation_noise_thresh + ) + ): + self.mean_error = error / error_cnt + self.additive_mean_error += self.mean_error + if np.abs(self.mean_error) < self.param.max_drift: + self.elevation_map[0] += self.mean_error * self.param.drift_compensation_alpha + self.add_points_kernel( + cp.array([0.0], dtype=self.data_type), + cp.array([0.0], dtype=self.data_type), + R, + t, + self.normal_map, + points, + self.elevation_map, + self.new_map, + size=(points.shape[0]), + ) + self.average_map_kernel(self.new_map, self.elevation_map, size=(self.cell_n * self.cell_n)) + + self.semantic_map.update_layers_pointcloud(points_all, channels, R, t, self.new_map) + + if self.param.enable_overlap_clearance: + self.clear_overlap_map(t) + # dilation before traversability_filter + self.traversability_input *= 0.0 + self.dilation_filter_kernel( + self.elevation_map[5], + self.elevation_map[2] + self.elevation_map[6], + self.traversability_input, + self.traversability_mask_dummy, + size=(self.cell_n * self.cell_n), + ) + # calculate traversability + traversability = self.traversability_filter(self.traversability_input) + self.elevation_map[3][3:-3, 3:-3] = traversability.reshape( + (traversability.shape[2], traversability.shape[3]) + ) + + # calculate normal vectors + self.update_normal(self.traversability_input) + + def clear_overlap_map(self, t): + """Clear overlapping areas around the map center. + + Args: + t (cupy._core.core.ndarray): Absolute point position + """ + + height_min = t[2] - self.param.overlap_clear_range_z + height_max = t[2] + self.param.overlap_clear_range_z + near_map = self.elevation_map[:, self.cell_min : self.cell_max, self.cell_min : self.cell_max] + valid_idx = ~cp.logical_or(near_map[0] < height_min, near_map[0] > height_max) + near_map[0] = cp.where(valid_idx, near_map[0], 0.0) + near_map[1] = cp.where(valid_idx, near_map[1], self.initial_variance) + near_map[2] = cp.where(valid_idx, near_map[2], 0.0) + valid_idx = ~cp.logical_or(near_map[5] < height_min, near_map[5] > height_max) + near_map[5] = cp.where(valid_idx, near_map[5], 0.0) + near_map[6] = cp.where(valid_idx, near_map[6], 0.0) + self.elevation_map[:, self.cell_min : self.cell_max, self.cell_min : self.cell_max] = near_map + + def get_additive_mean_error(self): + """Returns the additive mean error. + + Returns: + + """ + return self.additive_mean_error + + def update_variance(self): + """Adds the time variacne to the valid cells.""" + self.elevation_map[1] += self.param.time_variance * self.elevation_map[2] + + def update_time(self): + """adds the time interval to the time layer.""" + self.elevation_map[4] += self.param.time_interval + + def update_upper_bound_with_valid_elevation(self): + """Filters all invalid cell's upper_bound and is_upper_bound layers.""" + mask = self.elevation_map[2] > 0.5 + self.elevation_map[5] = cp.where(mask, self.elevation_map[0], self.elevation_map[5]) + self.elevation_map[6] = cp.where(mask, 0.0, self.elevation_map[6]) + + def input_pointcloud( + self, + raw_points: cp._core.core.ndarray, + channels: List[str], + R: cp._core.core.ndarray, + t: cp._core.core.ndarray, + position_noise: float, + orientation_noise: float, + ): + """Input the point cloud and fuse the new measurements to update the elevation map. + + Args: + raw_points (cupy._core.core.ndarray): + channels (List[str]): + R (cupy._core.core.ndarray): + t (cupy._core.core.ndarray): + position_noise (float): + orientation_noise (float): + + Returns: + None: + """ + raw_points = cp.asarray(raw_points, dtype=self.data_type) + additional_channels = channels[3:] + raw_points = raw_points[~cp.isnan(raw_points[:, :3]).any(axis=1)] + self.update_map_with_kernel( + raw_points, + additional_channels, + cp.asarray(R, dtype=self.data_type), + cp.asarray(t, dtype=self.data_type), + position_noise, + orientation_noise, + ) + + def input_image( + self, + image: List[cp._core.core.ndarray], + channels: List[str], + # fusion_methods: List[str], + R: cp._core.core.ndarray, + t: cp._core.core.ndarray, + K: cp._core.core.ndarray, + D: cp._core.core.ndarray, + distortion_model: str, + image_height: int, + image_width: int, + ): + """Input image and fuse the new measurements to update the elevation map. + + Args: + sub_key (str): Key used to identify the subscriber configuration + image (List[cupy._core.core.ndarray]): List of array containing the individual image input channels + R (cupy._core.core.ndarray): Camera optical center rotation + t (cupy._core.core.ndarray): Camera optical center translation + K (cupy._core.core.ndarray): Camera intrinsics + image_height (int): Image height + image_width (int): Image width + + Returns: + None: + """ + + image = np.stack(image, axis=0) + if len(image.shape) == 2: + image = image[None] + + # Convert to cupy + image = cp.asarray(image, dtype=self.data_type) + K = cp.asarray(K, dtype=self.data_type) + R = cp.asarray(R, dtype=self.data_type) + t = cp.asarray(t, dtype=self.data_type) + D = cp.asarray(D, dtype=self.data_type) + image_height = cp.float32(image_height) + image_width = cp.float32(image_width) + + if len(D) < 4: + D = cp.zeros(5, dtype=self.data_type) + elif len(D) == 4: + D = cp.concatenate([D, cp.zeros(1, dtype=self.data_type)]) + else: + D = D[:5] + + if distortion_model == "radtan": + pass + elif distortion_model == "equidistant": + # Not implemented yet. + D *= 0 + elif distortion_model == "plumb_bob": + # Not implemented yet. + D *= 0 + else: + # Not implemented yet. + D *= 0 + + # Calculate transformation matrix + P = cp.asarray(K @ cp.concatenate([R, t[:, None]], 1), dtype=np.float32) + t_cam_map = -R.T @ t - self.center + t_cam_map = t_cam_map.get() + x1 = cp.uint32((self.cell_n / 2) + ((t_cam_map[0]) / self.resolution)) + y1 = cp.uint32((self.cell_n / 2) + ((t_cam_map[1]) / self.resolution)) + z1 = cp.float32(t_cam_map[2]) + + self.uv_correspondence *= 0 + self.valid_correspondence[:, :] = False + + with self.map_lock: + self.image_to_map_correspondence_kernel( + self.elevation_map, + x1, + y1, + z1, + P.reshape(-1), + K.reshape(-1), + D.reshape(-1), + image_height, + image_width, + self.center, + self.uv_correspondence, + self.valid_correspondence, + size=int(self.cell_n * self.cell_n), + ) + self.semantic_map.update_layers_image( + image, + channels, + self.uv_correspondence, + self.valid_correspondence, + image_height, + image_width, + ) + + def update_normal(self, dilated_map): + """Clear the normal map and then apply the normal kernel with dilated map as input. + + Args: + dilated_map (cupy._core.core.ndarray): + """ + with self.map_lock: + self.normal_map *= 0.0 + self.normal_filter_kernel( + dilated_map, + self.elevation_map[2], + self.normal_map, + size=(self.cell_n * self.cell_n), + ) + + def process_map_for_publish(self, input_map, fill_nan=False, add_z=False, xp=cp): + """Process the input_map according to the fill_nan and add_z flags. + + Args: + input_map (cupy._core.core.ndarray): + fill_nan (bool): + add_z (bool): + xp (module): + + Returns: + cupy._core.core.ndarray: + """ + m = input_map.copy() + if fill_nan: + m = xp.where(self.elevation_map[2] > 0.5, m, xp.nan) + if add_z: + m = m + self.center[2] + return m[1:-1, 1:-1] + + def get_elevation(self): + """Get the elevation layer. + + Returns: + elevation layer + + """ + return self.process_map_for_publish(self.elevation_map[0], fill_nan=True, add_z=True) + + def get_variance(self): + """Get the variance layer. + + Returns: + variance layer + """ + return self.process_map_for_publish(self.elevation_map[1], fill_nan=False, add_z=False) + + def get_traversability(self): + """Get the traversability layer. + + Returns: + traversability layer + """ + traversability = cp.where( + (self.elevation_map[2] + self.elevation_map[6]) > 0.5, + self.elevation_map[3].copy(), + cp.nan, + ) + self.traversability_buffer[3:-3, 3:-3] = traversability[3:-3, 3:-3] + traversability = self.traversability_buffer[1:-1, 1:-1] + return traversability + + def get_time(self): + """Get the time layer. + + Returns: + time layer + """ + return self.process_map_for_publish(self.elevation_map[4], fill_nan=False, add_z=False) + + def get_upper_bound(self): + """Get the upper bound layer. + + Returns: + upper_bound: upper bound layer + """ + if self.param.use_only_above_for_upper_bound: + valid = cp.logical_or( + cp.logical_and(self.elevation_map[5] > 0.0, self.elevation_map[6] > 0.5), + self.elevation_map[2] > 0.5, + ) + else: + valid = cp.logical_or(self.elevation_map[2] > 0.5, self.elevation_map[6] > 0.5) + upper_bound = cp.where(valid, self.elevation_map[5].copy(), cp.nan) + upper_bound = upper_bound[1:-1, 1:-1] + self.center[2] + return upper_bound + + def get_is_upper_bound(self): + """Get the is upper bound layer. + + Returns: + is_upper_bound: layer + """ + if self.param.use_only_above_for_upper_bound: + valid = cp.logical_or( + cp.logical_and(self.elevation_map[5] > 0.0, self.elevation_map[6] > 0.5), + self.elevation_map[2] > 0.5, + ) + else: + valid = cp.logical_or(self.elevation_map[2] > 0.5, self.elevation_map[6] > 0.5) + is_upper_bound = cp.where(valid, self.elevation_map[6].copy(), cp.nan) + is_upper_bound = is_upper_bound[1:-1, 1:-1] + return is_upper_bound + + def xp_of_array(self, array): + """Indicate which library is used for xp. + + Args: + array (cupy._core.core.ndarray): + + Returns: + module: either np or cp + """ + if type(array) == cp.ndarray: + return cp + elif type(array) == np.ndarray: + return np + + def copy_to_cpu(self, array, data, stream=None): + """Transforms the data to float32 and if on gpu loads it to cpu. + + Args: + array (cupy._core.core.ndarray): + data (numpy.ndarray): + stream (Union[None, cupy.cuda.stream.Stream, None, None, None, None, None, None, None]): + """ + if type(array) == np.ndarray: + data[...] = array.astype(np.float32) + elif type(array) == cp.ndarray: + if stream is not None: + data[...] = cp.asnumpy(array.astype(np.float32), stream=stream) + else: + data[...] = cp.asnumpy(array.astype(np.float32)) + + def exists_layer(self, name): + """Check if the layer exists in elevation map or in the semantic map. + + Args: + name (str): Layer name + + Returns: + bool: Indicates if layer exists. + """ + if name in self.layer_names: + return True + elif name in self.semantic_map.layer_names: + return True + elif name in self.plugin_manager.layer_names: + return True + else: + return False + + def get_map_with_name_ref(self, name, data): + """Load a layer according to the name input to the data input. + + Args: + name (str): Name of the layer. + data (numpy.ndarray): Data structure that contains layer. + + """ + use_stream = True + xp = cp + with self.map_lock: + if name == "elevation": + m = self.get_elevation() + use_stream = False + elif name == "variance": + m = self.get_variance() + elif name == "traversability": + m = self.get_traversability() + elif name == "time": + m = self.get_time() + elif name == "upper_bound": + m = self.get_upper_bound() + elif name == "is_upper_bound": + m = self.get_is_upper_bound() + elif name == "normal_x": + m = self.normal_map.copy()[0, 1:-1, 1:-1] + elif name == "normal_y": + m = self.normal_map.copy()[1, 1:-1, 1:-1] + elif name == "normal_z": + m = self.normal_map.copy()[2, 1:-1, 1:-1] + elif name in self.semantic_map.layer_names: + m = self.semantic_map.get_map_with_name(name) + elif name in self.plugin_manager.layer_names: + self.plugin_manager.update_with_name( + name, + self.elevation_map, + self.layer_names, + self.semantic_map.semantic_map, + self.semantic_map.layer_names, + self.base_rotation, + self.semantic_map.elements_to_shift, + ) + m = self.plugin_manager.get_map_with_name(name) + p = self.plugin_manager.get_param_with_name(name) + xp = self.xp_of_array(m) + m = self.process_map_for_publish(m, fill_nan=p.fill_nan, add_z=p.is_height_layer, xp=xp) + else: + print("Layer {} is not in the map".format(name)) + return + m = xp.flip(m, 0) + m = xp.flip(m, 1) + if use_stream: + stream = cp.cuda.Stream(non_blocking=False) + else: + stream = None + self.copy_to_cpu(m, data, stream=stream) + + def get_normal_maps(self): + """Get the normal maps. + + Returns: + maps: the three normal values for each cell + """ + normal = self.normal_map.copy() + normal_x = normal[0, 1:-1, 1:-1] + normal_y = normal[1, 1:-1, 1:-1] + normal_z = normal[2, 1:-1, 1:-1] + maps = xp.stack([normal_x, normal_y, normal_z], axis=0) + maps = xp.flip(maps, 1) + maps = xp.flip(maps, 2) + maps = xp.asnumpy(maps) + return maps + + def get_normal_ref(self, normal_x_data, normal_y_data, normal_z_data): + """Get the normal maps as reference. + + Args: + normal_x_data: + normal_y_data: + normal_z_data: + """ + maps = self.get_normal_maps() + self.stream = cp.cuda.Stream(non_blocking=True) + normal_x_data[...] = xp.asnumpy(maps[0], stream=self.stream) + normal_y_data[...] = xp.asnumpy(maps[1], stream=self.stream) + normal_z_data[...] = xp.asnumpy(maps[2], stream=self.stream) + + def get_layer(self, name): + """Return the layer with the name input. + + Args: + name: The layers name. + + Returns: + return_map: The rqeuested layer. + + """ + if name in self.layer_names: + idx = self.layer_names.index(name) + return_map = self.elevation_map[idx] + elif name in self.semantic_map.layer_names: + idx = self.semantic_map.layer_names.index(name) + return_map = self.semantic_map.semantic_map[idx] + elif name in self.plugin_manager.layer_names: + self.plugin_manager.update_with_name( + name, + self.elevation_map, + self.layer_names, + self.semantic_map, + self.base_rotation, + ) + return_map = self.plugin_manager.get_map_with_name(name) + else: + print("Layer {} is not in the map, returning traversabiltiy!".format(name)) + return + return return_map + + def get_polygon_traversability(self, polygon, result): + """Check if input polygons are traversable. + + Args: + polygon (cupy._core.core.ndarray): + result (numpy.ndarray): + + Returns: + Union[None, int]: + """ + polygon = xp.asarray(polygon) + area = calculate_area(polygon) + polygon = polygon.astype(self.data_type) + pmin = self.center[:2] - self.map_length / 2 + self.resolution + pmax = self.center[:2] + self.map_length / 2 - self.resolution + polygon[:, 0] = polygon[:, 0].clip(pmin[0], pmax[0]) + polygon[:, 1] = polygon[:, 1].clip(pmin[1], pmax[1]) + polygon_min = polygon.min(axis=0) + polygon_max = polygon.max(axis=0) + polygon_bbox = cp.concatenate([polygon_min, polygon_max]).flatten() + polygon_n = xp.array(polygon.shape[0], dtype=np.int16) + clipped_area = calculate_area(polygon) + self.polygon_mask_kernel( + polygon, + self.center[0], + self.center[1], + polygon_n, + polygon_bbox, + self.mask, + size=(self.cell_n * self.cell_n), + ) + tmp_map = self.get_layer(self.param.checker_layer) + masked, masked_isvalid = get_masked_traversability(self.elevation_map, self.mask, tmp_map) + if masked_isvalid.sum() > 0: + t = masked.sum() / masked_isvalid.sum() + else: + t = cp.asarray(0.0, dtype=self.data_type) + is_safe, un_polygon = is_traversable( + masked, + self.param.safe_thresh, + self.param.safe_min_thresh, + self.param.max_unsafe_n, + ) + untraversable_polygon_num = 0 + if un_polygon is not None: + un_polygon = transform_to_map_position(un_polygon, self.center[:2], self.cell_n, self.resolution) + untraversable_polygon_num = un_polygon.shape[0] + if clipped_area < 0.001: + is_safe = False + print("requested polygon is outside of the map") + result[...] = np.array([is_safe, t.get(), area.get()]) + self.untraversable_polygon = un_polygon + return untraversable_polygon_num + + def get_untraversable_polygon(self, untraversable_polygon): + """Copy the untraversable polygons to input untraversable_polygons. + + Args: + untraversable_polygon (numpy.ndarray): + """ + untraversable_polygon[...] = xp.asnumpy(self.untraversable_polygon) + + def initialize_map(self, points, method="cubic"): + """Initializes the map according to some points and using an approximation according to method. + + Args: + points (numpy.ndarray): + method (str): Interpolation method ['linear', 'cubic', 'nearest'] + """ + self.clear() + with self.map_lock: + points = cp.asarray(points, dtype=self.data_type) + indices = transform_to_map_index(points[:, :2], self.center[:2], self.cell_n, self.resolution) + points[:, :2] = indices.astype(points.dtype) + points[:, 2] -= self.center[2] + self.map_initializer(self.elevation_map, points, method) + if self.param.dilation_size_initialize > 0: + for i in range(2): + self.dilation_filter_kernel_initializer( + self.elevation_map[0], + self.elevation_map[2], + self.elevation_map[0], + self.elevation_map[2], + size=(self.cell_n * self.cell_n), + ) + self.update_upper_bound_with_valid_elevation() + + +if __name__ == "__main__": + # Test script for profiling. + # $ python -m cProfile -o profile.stats elevation_mapping.py + # $ snakeviz profile.stats + xp.random.seed(123) + R = xp.random.rand(3, 3) + t = xp.random.rand(3) + print(R, t) + param = Parameter( + use_chainer=False, + weight_file="/home/hojin/new_elev/src/elevation_mapping_cupy/elevation_mapping_cupy/config/core/weights.dat", + plugin_config_file="/home/hojin/new_elev/src/elevation_mapping_cupy/elevation_mapping_cupy/config/core/plugin_config.yaml", + ) + param.additional_layers = ["rgb", "grass", "tree", "people"] + # param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"] + param.fusion_algorithms = ["image_color", "pointcloud_color", "pointcloud_class_average"] + param.update() + elevation = ElevationMap(param) + layers = [ + "elevation", + "variance", + "traversability", + "min_filter", + "smooth", + "inpaint", + "rgb", + ] + points = xp.random.rand(100000, len(layers)) + + channels = ["x", "y", "z"] + param.additional_layers + print(channels) + data = np.zeros((elevation.cell_n - 2, elevation.cell_n - 2), dtype=np.float32) + for i in range(50): + elevation.input_pointcloud(points, channels, R, t, 0, 0) + elevation.update_normal(elevation.elevation_map[0]) + pos = np.array([i * 0.01, i * 0.02, i * 0.01]) + elevation.move_to(pos, R) + for layer in layers: + elevation.get_map_with_name_ref(layer, data) + print(i) + polygon = cp.array([[0, 0], [2, 0], [0, 2]], dtype=param.data_type) + result = np.array([0, 0, 0]) + elevation.get_polygon_traversability(polygon, result) + print(result) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping_ros.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping_ros.py new file mode 100644 index 00000000..94cc1e5c --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping_ros.py @@ -0,0 +1,322 @@ +# # General +# import os +# import numpy as np +# import yaml + +# from elevation_mapping_cupy import ElevationMap, Parameter + + + +# # ROS2 +# import rclpy +# from rclpy.node import Node +# from tf_transformations import quaternion_matrix +# import tf2_ros +# from ament_index_python.packages import get_package_share_directory +# from cv_bridge import CvBridge + +# from sensor_msgs.msg import PointCloud2, Image, CameraInfo +# from grid_map_msgs.msg import GridMap +# from std_msgs.msg import Float32MultiArray, MultiArrayLayout as MAL, MultiArrayDimension as MAD + +# from functools import partial +# import message_filters + + +# PDC_DATATYPE = { +# "1": np.int8, +# "2": np.uint8, +# "3": np.int16, +# "4": np.uint16, +# "5": np.int32, +# "6": np.uint32, +# "7": np.float32, +# "8": np.float64, +# } + + +# class ElevationMapWrapper(Node): +# def __init__(self): +# super().__init__('elevation_mapping') +# self.node_name = "elevation_mapping" +# self.root = get_package_share_directory("elevation_mapping_cupy") +# weight_file = os.path.join(self.root, "config/core/weights.dat") +# plugin_config_file = os.path.join(self.root, "config/core/plugin_config.yaml") +# self.param = Parameter(use_chainer=False, weight_file=weight_file, plugin_config_file=plugin_config_file) + +# # ROS2 +# self.initialize_ros() +# self.param.subscriber_cfg = self.subscribers + +# self.initialize_elevation_mapping() +# # self.register_subscribers() +# # self.register_publishers() +# # self.register_timers() + +# self._last_t = None + + + + +# # # ROS +# # self.initalize_ros() +# # self.param.subscriber_cfg = self.subscribers + + +# def initialize_ros(self): +# self._tf_buffer = tf2_ros.Buffer() +# self._listener = tf2_ros.TransformListener(self._tf_buffer, self) +# self.get_ros_params() + + +# def initialize_elevation_mapping(self): +# self.param.update() +# # TODO add statistics across processed topics +# self._pointcloud_process_counter = 0 +# self._image_process_counter = 0 +# self._map = ElevationMap(self.param) +# self._map_data = np.zeros((self._map.cell_n - 2, self._map.cell_n - 2), dtype=np.float32) +# self._map_q = None +# self._map_t = None + + + + +# # def register_subscribers(self): +# # # check if CV bridge is needed +# # for config in self.param.subscriber_cfg.values(): +# # if config["data_type"] == "image": +# # self.cv_bridge = CvBridge() +# # break + +# # pointcloud_subs = {} +# # image_subs = {} +# # for key, config in self.subscribers.items(): +# # if config["data_type"] == "image": +# # camera_sub = message_filters.Subscriber(config["topic_name_camera"], Image) +# # camera_info_sub = message_filters.Subscriber(config["topic_name_camera_info"], CameraInfo) +# # image_subs[key] = message_filters.ApproximateTimeSynchronizer( +# # [camera_sub, camera_info_sub], queue_size=10, slop=0.5 +# # ) +# # image_subs[key].registerCallback(self.image_callback, key) + +# # elif config["data_type"] == "pointcloud": +# # pointcloud_subs[key] = rospy.Subscriber( +# # config["topic_name"], PointCloud2, self.pointcloud_callback, key +# # ) + +# # def register_publishers(self): +# # self._publishers = {} +# # self._publishers_timers = [] +# # for k, v in self.publishers.items(): +# # self._publishers[k] = rospy.Publisher(f"/{self.node_name}/{k}", GridMap, queue_size=10) +# # # partial(.) allows to pass a default argument to a callback +# # self._publishers_timers.append(rospy.Timer(rospy.Duration(1 / v["fps"]), partial(self.publish_map, k))) + +# # def publish_map(self, k, t): +# # print("publish_map") +# # if self._map_q is None: +# # return +# # gm = GridMap() +# # gm.info.header.frame_id = self.map_frame +# # gm.info.header.stamp = rospy.Time.now() +# # gm.info.header.seq = 0 +# # gm.info.resolution = self._map.resolution +# # gm.info.length_x = self._map.map_length +# # gm.info.length_y = self._map.map_length +# # gm.info.pose.position.x = self._map_t.x +# # gm.info.pose.position.y = self._map_t.y +# # gm.info.pose.position.z = 0.0 +# # gm.info.pose.orientation.w = 1.0 # self._map_q.w +# # gm.info.pose.orientation.x = 0.0 # self._map_q.x +# # gm.info.pose.orientation.y = 0.0 # self._map_q.y +# # gm.info.pose.orientation.z = 0.0 # self._map_q.z +# # gm.layers = [] +# # gm.basic_layers = self.publishers[k]["basic_layers"] +# # for i, layer in enumerate(self.publishers[k]["layers"]): +# # gm.layers.append(layer) +# # try: +# # self._map.get_map_with_name_ref(layer, self._map_data) +# # data = self._map_data.copy() +# # arr = Float32MultiArray() +# # arr.layout = MAL() +# # N = self._map_data.shape[0] +# # arr.layout.dim.append(MAD(label="column_index", size=N, stride=int(N * N))) +# # arr.layout.dim.append(MAD(label="row_index", size=N, stride=N)) +# # arr.data = tuple(np.ascontiguousarray(data.T).reshape(-1)) +# # gm.data.append(arr) +# # except: +# # if layer in gm.basic_layers: +# # print("Error: Missed Layer in basic layers") + +# # gm.outer_start_index = 0 +# # gm.inner_start_index = 0 + +# # self._publishers[k].publish(gm) + +# # def register_timers(self): +# # self.timer_variance = rospy.Timer(rospy.Duration(1 / self.update_variance_fps), self.update_variance) +# # self.timer_pose = rospy.Timer(rospy.Duration(1 / self.update_pose_fps), self.update_pose) +# # self.timer_time = rospy.Timer(rospy.Duration(self.time_interval), self.update_time) + +# # def image_callback(self, camera_msg, camera_info_msg, sub_key): +# # # get pose of image +# # ti = rospy.Time(secs=camera_msg.header.stamp.secs, nsecs=camera_msg.header.stamp.nsecs) +# # self._last_t = ti +# # try: +# # transform = self._tf_buffer.lookup_transform( +# # camera_msg.header.frame_id, self.map_frame, ti, rospy.Duration(1.0) +# # ) +# # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: +# # print("Error: image_callback:", e) +# # return + +# # t = transform.transform.translation +# # t = np.array([t.x, t.y, t.z]) +# # q = transform.transform.rotation +# # R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] + +# # semantic_img = self.cv_bridge.imgmsg_to_cv2(camera_msg, desired_encoding="passthrough") + +# # if len(semantic_img.shape) != 2: +# # semantic_img = [semantic_img[:, :, k] for k in range(3)] + +# # else: +# # semantic_img = [semantic_img] + +# # K = np.array(camera_info_msg.K, dtype=np.float32).reshape(3, 3) + +# # assert np.all(np.array(camera_info_msg.D) == 0.0), "Undistortion not implemented" +# # D = np.array(camera_info_msg.D, dtype=np.float32).reshape(5, 1) + +# # # process pointcloud +# # self._map.input_image(sub_key, semantic_img, R, t, K, D, camera_info_msg.height, camera_info_msg.width) +# # self._image_process_counter += 1 + +# # def pointcloud_callback(self, msg, sub_key): +# # channels = ["x", "y", "z"] + self.param.subscriber_cfg[sub_key]["channels"] + +# # points = ros_numpy.numpify(msg) +# # pts = np.empty((points.shape[0], 0)) +# # for ch in channels: +# # p = points[ch] +# # if len(p.shape) == 1: +# # p = p[:, None] +# # pts = np.append(pts, p, axis=1) + +# # # get pose of pointcloud +# # ti = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs) +# # self._last_t = ti +# # try: +# # transform = self._tf_buffer.lookup_transform(self.map_frame, msg.header.frame_id, ti, rospy.Duration(1.0)) +# # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: +# # print("Error: pointcloud_callback: ", e) +# # return + +# # t = transform.transform.translation +# # t = np.array([t.x, t.y, t.z]) +# # q = transform.transform.rotation +# # R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] + +# # # process pointcloud +# # self._map.input(pts, channels, R, t, 0, 0) +# # self._pointcloud_process_counter += 1 +# # print("Pointclouds processed: ", self._pointcloud_process_counter) + +# # def update_pose(self, t): +# # # get pose of base +# # # t = rospy.Time.now() +# # if self._last_t is None: +# # return +# # try: +# # transform = self._tf_buffer.lookup_transform( +# # self.map_frame, self.base_frame, self._last_t, rospy.Duration(1.0) +# # ) +# # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: +# # print("Error: update_pose error: ", e) +# # return +# # t = transform.transform.translation +# # trans = np.array([t.x, t.y, t.z]) +# # q = transform.transform.rotation +# # rot = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] +# # self._map.move_to(trans, rot) + +# # self._map_t = t +# # self._map_q = q + +# # def update_variance(self, t): +# # self._map.update_variance() + +# # def update_time(self, t): +# # self._map.update_time() + + + +# def declare_parameters_from_yaml(self, yaml_file): +# def declare_nested_parameters(parent_name, param_dict): +# for sub_name, sub_value in param_dict.items(): +# full_param_name = f"{parent_name}.{sub_name}" + +# if isinstance(sub_value, dict): +# declare_nested_parameters(full_param_name, sub_value) +# else: +# if "elevation_mapping.ros__parameters" in full_param_name: +# full_param_name = full_param_name.replace("elevation_mapping.ros__parameters.", "") +# self.declare_parameter(full_param_name, sub_value) + +# with open(yaml_file, 'r') as file: +# params = yaml.safe_load(file) + +# # Set parameters in the node +# for param_name, param_value in params.items(): +# if isinstance(param_value, dict): +# # For nested dictionaries, set each nested parameter individually +# declare_nested_parameters(param_name, param_value) +# else: +# # For top-level parameters +# if "elevation_mapping.ros__parameters" in param_name: +# param_name = param_name.replace("elevation_mapping.ros__parameters.", "") +# self.declare_parameter(param_name, param_value) + + + + +# def get_ros_params(self): +# # TODO fix this here when later launching with launch-file +# # This is currently {p} elevation_mapping") +# param_file = os.path.join(self.root, f"config/core/core_param.yaml") +# self.declare_parameters_from_yaml(param_file) + +# # os.system(f"rosparam delete /{self.node_name}") +# # os.system(f"rosparam load {para} elevation_mapping") +# # self.subscribers = self.get_parameter("subscribers").get_parameter_value().string_value +# # self.publishers = self.get_parameter("publishers").get_parameter_value().string_value +# self.initialize_frame_id = self.get_parameter("initialize_frame_id").get_parameter_value().string_array_value +# self.initialize_tf_offset = self.get_parameter("initialize_tf_offset").get_parameter_value().double_value +# self.pose_topic = self.get_parameter("pose_topic").get_parameter_value().string_value +# self.map_frame = self.get_parameter("map_frame").get_parameter_value().string_value +# self.base_frame = self.get_parameter("base_frame").get_parameter_value().string_value +# self.corrected_map_frame = self.get_parameter("corrected_map_frame").get_parameter_value().string_value +# self.initialize_method = self.get_parameter("initialize_method").get_parameter_value().string_value +# self.position_lowpass_alpha = self.get_parameter("position_lowpass_alpha").get_parameter_value().double_value +# self.orientation_lowpass_alpha = self.get_parameter("orientation_lowpass_alpha").get_parameter_value().double_value +# self.update_variance_fps = self.get_parameter("update_variance_fps").get_parameter_value().double_value +# self.time_interval = self.get_parameter("time_interval").get_parameter_value().double_value +# self.update_pose_fps = self.get_parameter("update_pose_fps").get_parameter_value().double_value +# self.initialize_tf_grid_size = self.get_parameter("initialize_tf_grid_size").get_parameter_value().double_value +# self.map_acquire_fps = self.get_parameter("map_acquire_fps").get_parameter_value().double_value +# self.publish_statistics_fps = self.get_parameter("publish_statistics_fps").get_parameter_value().double_value +# self.enable_pointcloud_publishing = self.get_parameter("enable_pointcloud_publishing").get_parameter_value().bool_value +# self.enable_normal_arrow_publishing = self.get_parameter("enable_normal_arrow_publishing").get_parameter_value().bool_value +# self.enable_drift_corrected_TF_publishing = self.get_parameter("enable_drift_corrected_TF_publishing").get_parameter_value().bool_value +# self.use_initializer_at_start = self.get_parameter("use_initializer_at_start").get_parameter_value().bool_value + + +# def main(args=None): +# rclpy.init(args=args) +# node = ElevationMapWrapper() +# rclpy.spin(node) +# rclpy.shutdown() + +# if __name__ == '__main__': +# main() \ No newline at end of file diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/__init__.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/fusion_manager.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/fusion_manager.py new file mode 100644 index 00000000..aca34a6d --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/fusion_manager.py @@ -0,0 +1,112 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +from abc import ABC, abstractmethod +import cupy as cp +from typing import List, Dict, Any +from dataclasses import dataclass +import importlib +import inspect + + +# @dataclass +# class FusionParams: +# name: str + + +class FusionBase(ABC): + """ + This is a base class of Fusion + """ + + @abstractmethod + def __init__(self, *args, **kwargs): + """ + + Args: + fusion_params : FusionParams + The parameter of callback + """ + self.name = None + + @abstractmethod + def __call__(self, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map): + pass + + +class FusionManager(object): + def __init__(self, params): + self.fusion_plugins: Dict[str, FusionBase] = {} + self.params = params + self.plugins = [] + + def register_plugin(self, plugin): + """ + Register a new fusion plugin + """ + try: + m = importlib.import_module("." + plugin, package="elevation_mapping_cupy.fusion") # -> 'module' + except: + raise ValueError("Plugin {} does not exist.".format(plugin)) + for name, obj in inspect.getmembers(m): + + if inspect.isclass(obj) and issubclass(obj, FusionBase) and name != "FusionBase": + self.plugins.append(obj(self.params)) + + def get_plugin_idx(self, name: str, data_type: str): + """ + Get a registered fusion plugin + """ + name = data_type + "_" + name + for idx, plugin in enumerate(self.plugins): + if plugin.name == name: + return idx + print("[WARNING] Plugin {} is not in the list: {}".format(name, self.plugins)) + return None + + def execute_plugin( + self, name: str, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, elements_to_shift + ): + """ + Execute a registered fusion plugin + """ + idx = self.get_plugin_idx(name, "pointcloud") + if idx is not None: + self.plugins[idx]( + points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, elements_to_shift + ) + # else: + # raise ValueError("Plugin {} is not registered".format(name)) + + def execute_image_plugin( + self, + name: str, + sem_map_idx, + image, + j, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + semantic_map, + new_map, + ): + """ + Execute a registered fusion plugin + """ + idx = self.get_plugin_idx(name, "image") + if idx is not None: + self.plugins[idx]( + sem_map_idx, + image, + j, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + semantic_map, + new_map, + ) + # else: + # raise ValueError("Plugin {} is not registered".format(name)) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_color.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_color.py new file mode 100644 index 00000000..c882c1b0 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_color.py @@ -0,0 +1,86 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def color_correspondences_to_map_kernel(resolution, width, height): + color_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_rgb, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + + int idx_red = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + int idx_green = image_width * image_height + idx_red; + int idx_blue = image_width * image_height * 2 + idx_red; + + unsigned int r = image_rgb[idx_red]; + unsigned int g = image_rgb[idx_green]; + unsigned int b = image_rgb[idx_blue]; + + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + new_sem_map[get_map_idx(i, map_idx)] = rgb_; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + """ + ).substitute(), + name="color_correspondences_to_map_kernel", + ) + return color_correspondences_to_map_kernel + + +class ImageColor(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "image_color" + self.cell_n = params.cell_n + self.resolution = params.resolution + + self.color_correspondences_to_map_kernel = color_correspondences_to_map_kernel( + resolution=self.resolution, width=self.cell_n, height=self.cell_n, + ) + + def __call__( + self, + sem_map_idx, + image, + j, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + semantic_map, + new_map, + ): + self.color_correspondences_to_map_kernel( + semantic_map, + cp.uint64(sem_map_idx), + image, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + new_map, + size=int(self.cell_n * self.cell_n), + ) + semantic_map[sem_map_idx] = new_map[sem_map_idx] diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_exponential.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_exponential.py new file mode 100644 index 00000000..26aa2f89 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/image_exponential.py @@ -0,0 +1,77 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def exponential_correspondences_to_map_kernel(resolution, width, height, alpha): + exponential_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)] * (1-${alpha}) + ${alpha} * image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(alpha=alpha), + name="exponential_correspondences_to_map_kernel", + ) + return exponential_correspondences_to_map_kernel + + +class ImageExponential(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "image_exponential" + self.cell_n = params.cell_n + self.resolution = params.resolution + + self.exponential_correspondences_to_map_kernel = exponential_correspondences_to_map_kernel( + resolution=self.resolution, width=self.cell_n, height=self.cell_n, alpha=0.7, + ) + + def __call__( + self, + sem_map_idx, + image, + j, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + semantic_map, + new_map, + ): + self.exponential_correspondences_to_map_kernel( + semantic_map, + sem_map_idx, + image[j], + uv_correspondence, + valid_correspondence, + image_height, + image_width, + new_map, + size=int(self.cell_n * self.cell_n), + ) + semantic_map[sem_map_idx] = new_map[sem_map_idx] diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_average.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_average.py new file mode 100644 index 00000000..c4165a54 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_average.py @@ -0,0 +1,113 @@ +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def sum_kernel( + resolution, width, height, +): + """Sums the semantic values of the classes for the exponentiala verage or for the average. + + Args: + resolution: + width: + height: + + Returns: + + """ + # input the list of layers, amount of channels can slo be input through kernel + sum_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map, raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, map_lay[layer])], feat); + } + } + """ + ).substitute(), + name="sum_kernel", + ) + return sum_kernel + + +def average_kernel( + width, height, +): + average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = feat; + } + """ + ).substitute(), + name="average_map_kernel", + ) + return average_kernel + + +class Average(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "pointcloud_average" + self.cell_n = params.cell_n + self.resolution = params.resolution + self.sum_kernel = sum_kernel(self.resolution, self.cell_n, self.cell_n,) + self.average_kernel = average_kernel(self.cell_n, self.cell_n,) + + def __call__(self, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, *args): + self.sum_kernel( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + semantic_map, + new_map, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + self.average_kernel( + new_map, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + elevation_map, + semantic_map, + size=(self.cell_n * self.cell_n * pcl_ids.shape[0]), + ) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_bayesian_inference.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_bayesian_inference.py new file mode 100644 index 00000000..95dc8aea --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_bayesian_inference.py @@ -0,0 +1,122 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def sum_compact_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_compact_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, layer)], feat); + } + } + """ + ).substitute(), + name="sum_compact_kernel", + ) + return sum_compact_kernel + + +def bayesian_inference_kernel( + width, height, +): + bayesian_inference_kernel = cp.ElementwiseKernel( + in_params=" raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U newmap, raw U sum_mean, raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat_ml = sum_mean[get_map_idx(id, layer)]/cnt; + U feat_old = map[get_map_idx(id, map_lay[layer])]; + U sigma_old = newmap[get_map_idx(id, map_lay[layer])]; + U sigma = 1.0; + U feat_new = sigma*feat_old /(cnt*sigma_old + sigma) +cnt*sigma_old *feat_ml /(cnt*sigma_old+sigma); + U sigma_new = sigma*sigma_old /(cnt*sigma_old +sigma); + map[get_map_idx(id, map_lay[layer])] = feat_new; + newmap[get_map_idx(id, map_lay[layer])] = sigma_new; + } + """ + ).substitute(), + name="bayesian_inference_kernel", + ) + return bayesian_inference_kernel + + +class BayesianInference(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize bayesian inference kernel") + self.name = "pointcloud_bayesian_inference" + + self.cell_n = params.cell_n + self.resolution = params.resolution + self.fusion_algorithms = params.fusion_algorithms + self.data_type = params.data_type + + self.sum_mean = cp.ones( + (self.fusion_algorithms.count("bayesian_inference"), self.cell_n, self.cell_n,), self.data_type, + ) + # TODO initialize the variance with a value different than 0 + self.sum_compact_kernel = sum_compact_kernel(self.resolution, self.cell_n, self.cell_n,) + self.bayesian_inference_kernel = bayesian_inference_kernel(self.cell_n, self.cell_n,) + + def __call__(self, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, *args): + self.sum_mean *= 0 + self.sum_compact_kernel( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + self.sum_mean, + size=(points_all.shape[0]), + ) + self.bayesian_inference_kernel( + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + elevation_map, + new_map, + self.sum_mean, + semantic_map, + size=(self.cell_n * self.cell_n), + ) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_average.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_average.py new file mode 100644 index 00000000..80c96dc4 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_average.py @@ -0,0 +1,126 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def sum_kernel( + resolution, width, height, +): + """Sums the semantic values of the classes for the exponentiala verage or for the average. + + Args: + resolution: + width: + height: + + Returns: + + """ + # input the list of layers, amount of channels can slo be input through kernel + sum_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map, raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, map_lay[layer])], feat); + } + } + """ + ).substitute(), + name="sum_kernel", + ) + return sum_kernel + + +def class_average_kernel( + width, height, alpha, +): + class_average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U prev_val = map[get_map_idx(id, map_lay[layer])]; + if (prev_val==0){ + U val = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + else{ + U val = ${alpha} *prev_val + (1-${alpha}) * newmap[get_map_idx(id, map_lay[layer])]/(cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + } + """ + ).substitute(alpha=alpha,), + name="class_average_kernel", + ) + return class_average_kernel + + +class ClassAverage(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "pointcloud_class_average" + self.cell_n = params.cell_n + self.resolution = params.resolution + self.average_weight = params.average_weight + + self.sum_kernel = sum_kernel(self.resolution, self.cell_n, self.cell_n,) + self.class_average_kernel = class_average_kernel(self.cell_n, self.cell_n, self.average_weight,) + + def __call__(self, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, *args): + self.sum_kernel( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + semantic_map, + new_map, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + self.class_average_kernel( + new_map, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + elevation_map, + semantic_map, + size=(self.cell_n * self.cell_n * pcl_ids.shape[0]), + ) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_bayesian.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_bayesian.py new file mode 100644 index 00000000..5fec3a7d --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_bayesian.py @@ -0,0 +1,75 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def alpha_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + alpha_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U theta_max = 0; + W arg_max = 0; + U theta = p[id * pcl_channels[0] + pcl_chan[layer]]; + if (theta >=theta_max){ + arg_max = map_lay[layer]; + theta_max = theta; + } + atomicAdd(&newmap[get_map_idx(idx, arg_max)], theta_max); + } + } + """ + ).substitute(), + name="alpha_kernel", + ) + return alpha_kernel + + +class ClassBayesian(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "pointcloud_class_bayesian" + self.cell_n = params.cell_n + self.resolution = params.resolution + self.alpha_kernel = alpha_kernel(self.resolution, self.cell_n, self.cell_n,) + + def __call__(self, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, *args): + self.alpha_kernel( + points_all, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + new_map, + size=(points_all.shape[0]), + ) + # calculate new thetas + sum_alpha = cp.sum(new_map[layer_ids], axis=0) + # do not divide by zero + sum_alpha[sum_alpha == 0] = 1 + semantic_map[layer_ids] = new_map[layer_ids] / cp.expand_dims(sum_alpha, axis=0) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_max.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_max.py new file mode 100644 index 00000000..e506571d --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/fusion/pointcloud_class_max.py @@ -0,0 +1,123 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +import string + +from .fusion_manager import FusionBase + + +def sum_max_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_max_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U max_pt, raw T max_id, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U idx = p[i * pcl_channels[0]]; + U valid = p[i * pcl_channels[0] + 1]; + U inside = p[i * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + // for every max value + for ( W it=0;it> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = ( color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid && inside){ + unsigned int color = __float_as_uint(p[id * pcl_channels[0] + pcl_chan[layer]]); + atomicAdd(&color_map[get_map_idx(idx, layer*3)], get_r(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3+1)], get_g(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3 + 2)], get_b(color)); + atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1); + } + """ + ).substitute(width=width), + name="add_color_kernel", + ) + return add_color_kernel + + +def color_average_kernel( + width, height, +): + color_average_kernel = cp.ElementwiseKernel( + in_params="raw V color_map, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = (color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + unsigned int cnt = color_map[get_map_idx(id, pcl_channels[1]*3)]; + if (cnt>0){ + // U prev_color = map[get_map_idx(id, map_lay[layer])]; + unsigned int r = color_map[get_map_idx(id, layer*3)]/(1*cnt); + unsigned int g = color_map[get_map_idx(id, layer*3+1)]/(1*cnt); + unsigned int b = color_map[get_map_idx(id, layer*3+2)]/(1*cnt); + //if (prev_color>=0){ + // unsigned int prev_r = get_r(prev_color); + // unsigned int prev_g = get_g(prev_color); + // unsigned int prev_b = get_b(prev_color); + // unsigned int r = prev_r/2 + color_map[get_map_idx(i, layer*3)]/(2*cnt); + // unsigned int g = prev_g/2 + color_map[get_map_idx(i, layer*3+1)]/(2*cnt); + // unsigned int b = prev_b/2 + color_map[get_map_idx(i, layer*3+2)]/(2*cnt); + //} + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + map[get_map_idx(id, map_lay[layer])] = rgb_; + } + """ + ).substitute(), + name="color_average_kernel", + ) + return color_average_kernel + + +class Color(FusionBase): + def __init__(self, params, *args, **kwargs): + # super().__init__(fusion_params, *args, **kwargs) + # print("Initialize fusion kernel") + self.name = "pointcloud_color" + self.cell_n = params.cell_n + self.resolution = params.resolution + + self.add_color_kernel = add_color_kernel(params.cell_n, params.cell_n,) + self.color_average_kernel = color_average_kernel(self.cell_n, self.cell_n) + + def __call__(self, points_all, R, t, pcl_ids, layer_ids, elevation_map, semantic_map, new_map, *args): + self.color_map = cp.zeros((1 + 3 * layer_ids.shape[0], self.cell_n, self.cell_n), dtype=cp.uint32,) + + points_all = points_all.astype(cp.float32) + self.add_color_kernel( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + self.color_map, + size=(points_all.shape[0]), + ) + self.color_average_kernel( + self.color_map, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + semantic_map, + size=(self.cell_n * self.cell_n), + ) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/__init__.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/__init__.py new file mode 100644 index 00000000..06eecc38 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/__init__.py @@ -0,0 +1,3 @@ +from .custom_image_kernels import * +from .custom_kernels import * +from .custom_semantic_kernels import * diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_image_kernels.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_image_kernels.py new file mode 100644 index 00000000..a020ba2e --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_image_kernels.py @@ -0,0 +1,271 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string + + +def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_collision): + """ + This function calculates the correspondence between the image and the map. + It takes in the resolution, width, height, and tolerance_z_collision as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _image_to_map_correspondence_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U K, raw U D, raw U image_height, raw U image_width, raw U center", + out_params="raw U uv_correspondence, raw B valid_correspondence", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ bool is_inside_map(int x, int y) { + return (x >= 0 && y >= 0 && x<${width} && y<${height}); + } + __device__ float get_l2_distance(int x0, int y0, int x1, int y1) { + float dx = x0-x1; + float dy = y0-y1; + return sqrt( dx*dx + dy*dy); + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + + // return if gridcell has no valid height + if (map[get_map_idx(i, 2)] != 1){ + return; + } + + // get current cell position + int y0 = i % ${width}; + int x0 = i / ${width}; + + // gridcell 3D point in worldframe TODO reverse x and y + float p1 = (x0-(${width}/2)) * ${resolution} + center[0]; + float p2 = (y0-(${height}/2)) * ${resolution} + center[1]; + float p3 = map[cell_idx] + center[2]; + + // reproject 3D point into image plane + float u = p1 * P[0] + p2 * P[1] + p3 * P[2] + P[3]; + float v = p1 * P[4] + p2 * P[5] + p3 * P[6] + P[7]; + float d = p1 * P[8] + p2 * P[9] + p3 * P[10] + P[11]; + + // filter point behind image plane + if (d <= 0) { + return; + } + u = u/d; + v = v/d; + + // Check if D is all zeros + bool is_D_zero = (D[0] == 0 && D[1] == 0 && D[2] == 0 && D[3] == 0 && D[4] == 0); + + // Apply undistortion using distortion matrix D if not all zeros + if (!is_D_zero) { + float k1 = D[0]; + float k2 = D[1]; + float p1 = D[2]; + float p2 = D[3]; + float k3 = D[4]; + float fx = K[0]; + float fy = K[4]; + float cx = K[2]; + float cy = K[5]; + float x = (u - cx) / fx; + float y = (v - cy) / fy; + float r2 = x * x + y * y; + float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + float u_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float v_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = fx * u_corrected + cx; + v = fy * v_corrected + cy; + } + + // filter point next to image plane + if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ + return; + } + + int y0_c = y0; + int x0_c = x0; + float total_dis = get_l2_distance(x0_c, y0_c, x1,y1); + float z0 = map[cell_idx]; + float delta_z = z1-z0; + + + // bresenham algorithm to iterate over cells in line between camera center and current gridmap cell + // https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + int dx = abs(x1-x0); + int sx = x0 < x1 ? 1 : -1; + int dy = -abs(y1 - y0); + int sy = y0 < y1 ? 1 : -1; + int error = dx + dy; + + bool is_valid = true; + + // iterate over all cells along line + while (1){ + // assumption we do not need to check the height for camera center cell + if (x0 == x1 && y0 == y1){ + break; + } + + // check if height is invalid + if (is_inside_map(x0,y0)){ + int idx = y0 + (x0 * ${width}); + if (map[get_map_idx(idx, 2)]){ + float dis = get_l2_distance(x0_c, y0_c, x0, y0); + float rayheight = z0 + ( dis / total_dis * delta_z); + if ( map[idx] - ${tolerance_z_collision} > rayheight){ + is_valid = false; + break; + } + } + } + + + // computation of next gridcell index in line + int e2 = 2 * error; + if (e2 >= dy){ + if(x0 == x1){ + break; + } + error = error + dy; + x0 = x0 + sx; + } + if (e2 <= dx){ + if (y0 == y1){ + break; + } + error = error + dx; + y0 = y0 + sy; + } + } + + // mark the correspondence + uv_correspondence[get_map_idx(i, 0)] = u; + uv_correspondence[get_map_idx(i, 1)] = v; + valid_correspondence[get_map_idx(i, 0)] = is_valid; + """ + ).substitute(height=height, width=width, resolution=resolution, tolerance_z_collision=tolerance_z_collision), + name="image_to_map_correspondence_kernel", + ) + return _image_to_map_correspondence_kernel + + +def average_correspondences_to_map_kernel(width, height): + """ + This function calculates the average correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _average_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(), + name="average_correspondences_to_map_kernel", + ) + return _average_correspondences_to_map_kernel + + +def exponential_correspondences_to_map_kernel(width, height, alpha): + """ + This function calculates the exponential correspondences to the map. + It takes in the width, height, and alpha as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _exponential_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)] * (1-${alpha}) + ${alpha} * image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(alpha=alpha), + name="exponential_correspondences_to_map_kernel", + ) + return _exponential_correspondences_to_map_kernel + + +def color_correspondences_to_map_kernel(width, height): + """ + This function calculates the color correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _color_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_rgb, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + + int idx_red = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + int idx_green = image_width * image_height + idx_red; + int idx_blue = image_width * image_height * 2 + idx_red; + + unsigned int r = image_rgb[idx_red]; + unsigned int g = image_rgb[idx_green]; + unsigned int b = image_rgb[idx_blue]; + + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + new_sem_map[get_map_idx(i, map_idx)] = rgb_; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + """ + ).substitute(), + name="color_correspondences_to_map_kernel", + ) + return _color_correspondences_to_map_kernel diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_kernels.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_kernels.py new file mode 100644 index 00000000..d7025270 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_kernels.py @@ -0,0 +1,685 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string + + +def map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + util_preamble = string.Template( + """ + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ int get_x_idx(float16 x, float16 center) { + int i = (x - center) / ${resolution} + 0.5 * ${width}; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + int i = (y - center) / ${resolution} + 0.5 * ${height}; + return i; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x == 0 || idx_x == ${width} - 1) { + return false; + } + if (idx_y == 0 || idx_y == ${height} - 1) { + return false; + } + return true; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ float transform_p(float16 x, float16 y, float16 z, + float16 r0, float16 r1, float16 r2, float16 t) { + return r0 * x + r1 * y + r2 * z + t; + } + __device__ float z_noise(float16 z){ + return ${sensor_noise_factor} * z * z; + } + + __device__ float point_sensor_distance(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = (x - sx) * (x - sx) + (y - sy) * (y - sy) + (z - sz) * (z - sz); + return d; + } + + __device__ bool is_valid(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = point_sensor_distance(x, y, z, sx, sy, sz); + float dxy = max(sqrt(x * x + y * y) - ${ramped_height_range_b}, 0.0); + if (d < ${min_valid_distance} * ${min_valid_distance}) { + return false; + } + else if (z - sz > dxy * ${ramped_height_range_a} + ${ramped_height_range_c} || z - sz > ${max_height_range}) { + return false; + } + else { + return true; + } + } + + __device__ float ray_vector(float16 tx, float16 ty, float16 tz, + float16 px, float16 py, float16 pz, + float16& rx, float16& ry, float16& rz){ + float16 vx = px - tx; + float16 vy = py - ty; + float16 vz = pz - tz; + float16 norm = sqrt(vx * vx + vy * vy + vz * vz); + if (norm > 0) { + rx = vx / norm; + ry = vy / norm; + rz = vz / norm; + } + else { + rx = 0; + ry = 0; + rz = 0; + } + return norm; + } + + __device__ float inner_product(float16 x1, float16 y1, float16 z1, + float16 x2, float16 y2, float16 z2) { + + float product = (x1 * x2 + y1 * y2 + z1 * z2); + return product; + } + + """ + ).substitute( + resolution=resolution, + width=width, + height=height, + sensor_noise_factor=sensor_noise_factor, + min_valid_distance=min_valid_distance, + max_height_range=max_height_range, + ramped_height_range_a=ramped_height_range_a, + ramped_height_range_b=ramped_height_range_b, + ramped_height_range_c=ramped_height_range_c, + ) + return util_preamble + + +def add_points_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + wall_num_thresh, + max_ray_length, + cleanup_step, + min_valid_distance, + max_height_range, + cleanup_cos_thresh, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + enable_edge_shaped=True, + enable_visibility_cleanup=True, +): + add_points_kernel = cp.ElementwiseKernel( + in_params="raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map", + out_params="raw U p, raw U map, raw T newmap", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (is_valid(x, y, z, t[0], t[1], t[2])) { + if (is_inside(idx)) { + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U num_points = newmap[get_map_idx(idx, 4)]; + if (abs(map_h - z) > (map_v * ${mahalanobis_thresh})) { + atomicAdd(&map[get_map_idx(idx, 1)], ${outlier_variance}); + } + else { + if (${enable_edge_shaped} && (num_points > ${wall_num_thresh}) && (z < map_h - map_v * ${mahalanobis_thresh} / num_points)) { + // continue; + } + else { + T new_h = (map_h * v + z * map_v) / (map_v + v); + T new_v = (map_v * v) / (map_v + v); + atomicAdd(&newmap[get_map_idx(idx, 0)], new_h); + atomicAdd(&newmap[get_map_idx(idx, 1)], new_v); + atomicAdd(&newmap[get_map_idx(idx, 2)], 1.0); + // is Valid + map[get_map_idx(idx, 2)] = 1; + // Time layer + map[get_map_idx(idx, 4)] = 0.0; + // Upper bound + map[get_map_idx(idx, 5)] = new_h; + map[get_map_idx(idx, 6)] = 0.0; + } + // visibility cleanup + } + } + } + if (${enable_visibility_cleanup}) { + float16 ray_x, ray_y, ray_z; + float16 ray_length = ray_vector(t[0], t[1], t[2], x, y, z, ray_x, ray_y, ray_z); + ray_length = min(ray_length, (float16)${max_ray_length}); + int last_nidx = -1; + for (float16 s=${ray_step}; s < ray_length; s+=${ray_step}) { + // iterate through ray + U nx = t[0] + ray_x * s; + U ny = t[1] + ray_y * s; + U nz = t[2] + ray_z * s; + int nidx = get_idx(nx, ny, center_x[0], center_y[0]); + if (last_nidx == nidx) {continue;} // Skip if we're still in the same cell + else {last_nidx = nidx;} + if (!is_inside(nidx)) {continue;} + + U nmap_h = map[get_map_idx(nidx, 0)]; + U nmap_v = map[get_map_idx(nidx, 1)]; + U nmap_valid = map[get_map_idx(nidx, 2)]; + // traversability + U nmap_trav = map[get_map_idx(nidx, 3)]; + // Time layer + U non_updated_t = map[get_map_idx(nidx, 4)]; + // upper bound + U nmap_upper = map[get_map_idx(nidx, 5)]; + U nmap_is_upper = map[get_map_idx(nidx, 6)]; + + // If point is close or is farther away than ray length, skip. + float16 d = (x - nx) * (x - nx) + (y - ny) * (y - ny) + (z - nz) * (z - nz); + if (d < 0.1 || !is_valid(x, y, z, t[0], t[1], t[2])) {continue;} + + // If invalid, do upper bound check, then skip + if (nmap_valid < 0.5) { + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + continue; + } + // If updated recently, skip + if (non_updated_t < 0.5) {continue;} + + if (nmap_h > nz + 0.01 - min(nmap_v, 1.0) * 0.05) { + // If ray and norm is vertical, skip + U norm_x = norm_map[get_map_idx(nidx, 0)]; + U norm_y = norm_map[get_map_idx(nidx, 1)]; + U norm_z = norm_map[get_map_idx(nidx, 2)]; + float product = inner_product(ray_x, ray_y, ray_z, norm_x, norm_y, norm_z); + if (fabs(product) < ${cleanup_cos_thresh}) {continue;} + U num_points = newmap[get_map_idx(nidx, 3)]; + if (num_points > ${wall_num_thresh} && non_updated_t < 1.0) {continue;} + + // Finally, this cell is penetrated by the ray. + atomicAdd(&map[get_map_idx(nidx, 2)], -${cleanup_step}/(ray_length / ${max_ray_length})); + atomicAdd(&map[get_map_idx(nidx, 1)], ${outlier_variance}); + // Do upper bound check. + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + } + } + } + p[i * 3]= idx; + p[i * 3 + 1] = is_valid(x, y, z, t[0], t[1], t[2]); + p[i * 3 + 2] = is_inside(idx); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + wall_num_thresh=wall_num_thresh, + ray_step=resolution / 2 ** 0.5, + max_ray_length=max_ray_length, + cleanup_step=cleanup_step, + cleanup_cos_thresh=cleanup_cos_thresh, + enable_edge_shaped=int(enable_edge_shaped), + enable_visibility_cleanup=int(enable_visibility_cleanup), + ), + name="add_points_kernel", + ) + return add_points_kernel + + +def error_counting_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + traversability_inlier, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + error_counting_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U p, raw U center_x, raw U center_y, raw U R, raw U t", + out_params="raw U newmap, raw T error, raw T error_cnt", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + // if (!is_valid(z, t[2])) {return;} + if (!is_valid(x, y, z, t[0], t[1], t[2])) {return;} + // if ((x - t[0]) * (x - t[0]) + (y - t[1]) * (y - t[1]) + (z - t[2]) * (z - t[2]) < 0.5) {return;} + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (!is_inside(idx)) { + return; + } + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U map_valid = map[get_map_idx(idx, 2)]; + U map_t = map[get_map_idx(idx, 3)]; + if (map_valid > 0.5 && (abs(map_h - z) < (map_v * ${mahalanobis_thresh})) + && map_v < ${outlier_variance} / 2.0 + && map_t > ${traversability_inlier}) { + T e = z - map_h; + atomicAdd(&error[0], e); + atomicAdd(&error_cnt[0], 1); + atomicAdd(&newmap[get_map_idx(idx, 3)], 1.0); + } + atomicAdd(&newmap[get_map_idx(idx, 4)], 1.0); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + traversability_inlier=traversability_inlier, + ), + name="error_counting_kernel", + ) + return error_counting_kernel + + +def average_map_kernel(width, height, max_variance, initial_variance): + average_map_kernel = cp.ElementwiseKernel( + in_params="raw U newmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U v = map[get_map_idx(i, 1)]; + U valid = map[get_map_idx(i, 2)]; + U new_h = newmap[get_map_idx(i, 0)]; + U new_v = newmap[get_map_idx(i, 1)]; + U new_cnt = newmap[get_map_idx(i, 2)]; + if (new_cnt > 0) { + if (new_v / new_cnt > ${max_variance}) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + else { + map[get_map_idx(i, 0)] = new_h / new_cnt; + map[get_map_idx(i, 1)] = new_v / new_cnt; + map[get_map_idx(i, 2)] = 1; + } + } + if (valid < 0.5) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + """ + ).substitute(max_variance=max_variance, initial_variance=initial_variance), + name="average_map_kernel", + ) + return average_map_kernel + + +def dilation_filter_kernel(width, height, dilation_size): + dilation_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap, raw U newmask", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + newmap[get_map_idx(i, 0)] = h; + if (valid < 0.5) { + U distance = 100; + U near_value = 0; + for (int dy = -${dilation_size}; dy <= ${dilation_size}; dy++) { + for (int dx = -${dilation_size}; dx <= ${dilation_size}; dx++) { + int idx = get_relative_map_idx(i, dx, dy, 0); + if (!is_inside(idx)) {continue;} + U valid = mask[idx]; + if(valid > 0.5 && dx + dy < distance) { + distance = dx + dy; + near_value = map[idx]; + } + } + } + if(distance < 100) { + newmap[get_map_idx(i, 0)] = near_value; + newmask[get_map_idx(i, 0)] = 1.0; + } + } + """ + ).substitute(dilation_size=dilation_size), + name="dilation_filter_kernel", + ) + return dilation_filter_kernel + + +def normal_filter_kernel(width, height, resolution): + normal_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + __device__ float resolution() { + return ${resolution}; + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + if (valid > 0.5) { + int idx_x = get_relative_map_idx(i, 1, 0, 0); + int idx_y = get_relative_map_idx(i, 0, 1, 0); + if (!is_inside(idx_x) || !is_inside(idx_y)) { return; } + float dzdx = (map[idx_x] - h); + float dzdy = (map[idx_y] - h); + float nx = -dzdy / resolution(); + float ny = -dzdx / resolution(); + float nz = 1; + float norm = sqrt((nx * nx) + (ny * ny) + 1); + newmap[get_map_idx(i, 0)] = nx / norm; + newmap[get_map_idx(i, 1)] = ny / norm; + newmap[get_map_idx(i, 2)] = nz / norm; + } + """ + ).substitute(), + name="normal_filter_kernel", + ) + return normal_filter_kernel + + +def polygon_mask_kernel(width, height, resolution): + polygon_mask_kernel = cp.ElementwiseKernel( + in_params="raw U polygon, raw U center_x, raw U center_y, raw int16 polygon_n, raw U polygon_bbox", + out_params="raw U mask", + preamble=string.Template( + """ + __device__ struct Point + { + int x; + int y; + }; + // Given three colinear points p, q, r, the function checks if + // point q lies on line segment 'pr' + __device__ bool onSegment(Point p, Point q, Point r) + { + if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && + q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) + return true; + return false; + } + // To find orientation of ordered triplet (p, q, r). + // The function returns following values + // 0 --> p, q and r are colinear + // 1 --> Clockwise + // 2 --> Counterclockwise + __device__ int orientation(Point p, Point q, Point r) + { + int val = (q.y - p.y) * (r.x - q.x) - + (q.x - p.x) * (r.y - q.y); + if (val == 0) return 0; // colinear + return (val > 0)? 1: 2; // clock or counterclock wise + } + // The function that returns true if line segment 'p1q1' + // and 'p2q2' intersect. + __device__ bool doIntersect(Point p1, Point q1, Point p2, Point q2) + { + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + // General case + if (o1 != o2 && o3 != o4) + return true; + // Special Cases + // p1, q1 and p2 are colinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + // p1, q1 and p2 are colinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + // p2, q2 and p1 are colinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + // p2, q2 and q1 are colinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + return false; // Doesn't fall in any of the above cases + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_idx_x(int idx){ + int idx_x = idx / ${width}; + return idx_x; + } + + __device__ int get_idx_y(int idx){ + int idx_y = idx % ${width}; + return idx_y; + } + + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ float16 round(float16 x) { + return (int)x + (int)(2 * (x - (int)x)); + } + __device__ int get_x_idx(float16 x, float16 center) { + const float resolution = ${resolution}; + const float width = ${width}; + int i = (x - center) / resolution + 0.5 * width; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + const float resolution = ${resolution}; + const float height = ${height}; + int i = (y - center) / resolution + 0.5 * height; + return i; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + // Point p = {get_idx_x(i, center_x[0]), get_idx_y(i, center_y[0])}; + Point p = {get_idx_x(i), get_idx_y(i)}; + Point extreme = {100000, p.y}; + int bbox_min_idx = get_idx(polygon_bbox[0], polygon_bbox[1], center_x[0], center_y[0]); + int bbox_max_idx = get_idx(polygon_bbox[2], polygon_bbox[3], center_x[0], center_y[0]); + Point bmin = {get_idx_x(bbox_min_idx), get_idx_y(bbox_min_idx)}; + Point bmax = {get_idx_x(bbox_max_idx), get_idx_y(bbox_max_idx)}; + if (p.x < bmin.x || p.x > bmax.x || p.y < bmin.y || p.y > bmax.y){ + mask[i] = 0; + return; + } + else { + int intersect_cnt = 0; + for (int j = 0; j < polygon_n[0]; j++) { + Point p1, p2; + int i1 = get_idx(polygon[j * 2 + 0], polygon[j * 2 + 1], center_x[0], center_y[0]); + p1.x = get_idx_x(i1); + p1.y = get_idx_y(i1); + int j2 = (j + 1) % polygon_n[0]; + int i2 = get_idx(polygon[j2 * 2 + 0], polygon[j2 * 2 + 1], center_x[0], center_y[0]); + p2.x = get_idx_x(i2); + p2.y = get_idx_y(i2); + if (doIntersect(p1, p2, p, extreme)) + { + // If the point 'p' is colinear with line segment 'i-next', + // then check if it lies on segment. If it lies, return true, + // otherwise false + if (orientation(p1, p, p2) == 0) { + if (onSegment(p1, p, p2)){ + mask[i] = 1; + return; + } + } + else if(((p1.y <= p.y) && (p2.y > p.y)) || ((p1.y > p.y) && (p2.y <= p.y))){ + intersect_cnt++; + } + } + } + if (intersect_cnt % 2 == 0) { mask[i] = 0; } + else { mask[i] = 1; } + } + """ + ).substitute(a=1), + name="polygon_mask_kernel", + ) + return polygon_mask_kernel + + +if __name__ == "__main__": + for i in range(10): + import random + + a = cp.zeros((100, 100)) + n = random.randint(3, 5) + + # polygon = cp.array([[-1, -1], [3, 4], [2, 4], [1, 3]], dtype=float) + polygon = cp.array( + [[(random.random() - 0.5) * 10, (random.random() - 0.5) * 10] for i in range(n)], dtype=float + ) + print(polygon) + polygon_min = polygon.min(axis=0) + polygon_max = polygon.max(axis=0) + polygon_bbox = cp.concatenate([polygon_min, polygon_max]).flatten() + polygon_n = polygon.shape[0] + print(polygon_bbox) + # polygon_bbox = cp.array([-5, -5, 5, 5], dtype=float) + polygon_mask = polygon_mask_kernel(100, 100, 0.1) + import time + + start = time.time() + polygon_mask(polygon, 0.0, 0.0, polygon_n, polygon_bbox, a, size=(100 * 100)) + print(time.time() - start) + import pylab as plt + + print(a) + plt.imshow(cp.asnumpy(a)) + plt.show() diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_semantic_kernels.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_semantic_kernels.py new file mode 100644 index 00000000..26db74c0 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/custom_semantic_kernels.py @@ -0,0 +1,375 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string + + +def sum_kernel( + resolution, width, height, +): + """Sums the semantic values of the classes for the exponentiala verage or for the average. + + Args: + resolution: + width: + height: + + Returns: + + """ + # input the list of layers, amount of channels can slo be input through kernel + sum_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map, raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, map_lay[layer])], feat); + } + } + """ + ).substitute(), + name="sum_kernel", + ) + return sum_kernel + + +def sum_compact_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_compact_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, layer)], feat); + } + } + """ + ).substitute(), + name="sum_compact_kernel", + ) + return sum_compact_kernel + + +def sum_max_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_max_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U max_pt, raw T max_id, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U idx = p[i * pcl_channels[0]]; + U valid = p[i * pcl_channels[0] + 1]; + U inside = p[i * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + // for every max value + for ( W it=0;it=theta_max){ + arg_max = map_lay[layer]; + theta_max = theta; + } + atomicAdd(&newmap[get_map_idx(idx, arg_max)], theta_max); + } + } + """ + ).substitute(), + name="alpha_kernel", + ) + return alpha_kernel + + +def average_kernel( + width, height, +): + average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = feat; + } + """ + ).substitute(), + name="average_map_kernel", + ) + return average_kernel + + +def bayesian_inference_kernel( + width, height, +): + bayesian_inference_kernel = cp.ElementwiseKernel( + in_params=" raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U newmap, raw U sum_mean, raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat_ml = sum_mean[get_map_idx(id, layer)]/cnt; + U feat_old = map[get_map_idx(id, map_lay[layer])]; + U sigma_old = newmap[get_map_idx(id, map_lay[layer])]; + U sigma = 1.0; + U feat_new = sigma*feat_old /(cnt*sigma_old + sigma) +cnt*sigma_old *feat_ml /(cnt*sigma_old+sigma); + U sigma_new = sigma*sigma_old /(cnt*sigma_old +sigma); + map[get_map_idx(id, map_lay[layer])] = feat_new; + newmap[get_map_idx(id, map_lay[layer])] = sigma_new; + } + """ + ).substitute(), + name="bayesian_inference_kernel", + ) + return bayesian_inference_kernel + + +def class_average_kernel( + width, height, alpha, +): + class_average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U prev_val = map[get_map_idx(id, map_lay[layer])]; + if (prev_val==0){ + U val = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + else{ + U val = ${alpha} *prev_val + (1-${alpha}) * newmap[get_map_idx(id, map_lay[layer])]/(cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + } + """ + ).substitute(alpha=alpha,), + name="class_average_kernel", + ) + return class_average_kernel + + +def add_color_kernel( + width, height, +): + add_color_kernel = cp.ElementwiseKernel( + in_params="raw T p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw V color_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = ( color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid && inside){ + unsigned int color = __float_as_uint(p[id * pcl_channels[0] + pcl_chan[layer]]); + atomicAdd(&color_map[get_map_idx(idx, layer*3)], get_r(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3+1)], get_g(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3 + 2)], get_b(color)); + atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1); + } + """ + ).substitute(width=width), + name="add_color_kernel", + ) + return add_color_kernel + + +def color_average_kernel( + width, height, +): + color_average_kernel = cp.ElementwiseKernel( + in_params="raw V color_map, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = (color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + unsigned int cnt = color_map[get_map_idx(id, pcl_channels[1]*3)]; + if (cnt>0){ + // U prev_color = map[get_map_idx(id, map_lay[layer])]; + unsigned int r = color_map[get_map_idx(id, layer*3)]/(1*cnt); + unsigned int g = color_map[get_map_idx(id, layer*3+1)]/(1*cnt); + unsigned int b = color_map[get_map_idx(id, layer*3+2)]/(1*cnt); + //if (prev_color>=0){ + // unsigned int prev_r = get_r(prev_color); + // unsigned int prev_g = get_g(prev_color); + // unsigned int prev_b = get_b(prev_color); + // unsigned int r = prev_r/2 + color_map[get_map_idx(i, layer*3)]/(2*cnt); + // unsigned int g = prev_g/2 + color_map[get_map_idx(i, layer*3+1)]/(2*cnt); + // unsigned int b = prev_b/2 + color_map[get_map_idx(i, layer*3+2)]/(2*cnt); + //} + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + map[get_map_idx(id, map_lay[layer])] = rgb_; + } + """ + ).substitute(), + name="color_average_kernel", + ) + return color_average_kernel diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/kk.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/kk.py new file mode 100644 index 00000000..27562dda --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/kernels/kk.py @@ -0,0 +1,1290 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string + + +def sum_kernel( + resolution, width, height, +): + """Sums the semantic values of the classes for the exponentiala verage or for the average. + + Args: + resolution: + width: + height: + + Returns: + + """ + # input the list of layers, amount of channels can slo be input through kernel + sum_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map, raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, map_lay[layer])], feat); + } + } + """ + ).substitute(), + name="sum_kernel", + ) + return sum_kernel + + +def sum_compact_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_compact_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, layer)], feat); + } + } + """ + ).substitute(), + name="sum_compact_kernel", + ) + return sum_compact_kernel + + +def sum_max_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_max_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U max_pt, raw T max_id, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U idx = p[i * pcl_channels[0]]; + U valid = p[i * pcl_channels[0] + 1]; + U inside = p[i * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + // for every max value + for ( W it=0;it=theta_max){ + arg_max = map_lay[layer]; + theta_max = theta; + } + atomicAdd(&newmap[get_map_idx(idx, arg_max)], theta_max); + } + } + """ + ).substitute(), + name="alpha_kernel", + ) + return alpha_kernel + + +def average_kernel( + width, height, +): + average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = feat; + } + """ + ).substitute(), + name="average_map_kernel", + ) + return average_kernel + + +def bayesian_inference_kernel( + width, height, +): + bayesian_inference_kernel = cp.ElementwiseKernel( + in_params=" raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U newmap, raw U sum_mean, raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat_ml = sum_mean[get_map_idx(id, layer)]/cnt; + U feat_old = map[get_map_idx(id, map_lay[layer])]; + U sigma_old = newmap[get_map_idx(id, map_lay[layer])]; + U sigma = 1.0; + U feat_new = sigma*feat_old /(cnt*sigma_old + sigma) +cnt*sigma_old *feat_ml /(cnt*sigma_old+sigma); + U sigma_new = sigma*sigma_old /(cnt*sigma_old +sigma); + map[get_map_idx(id, map_lay[layer])] = feat_new; + newmap[get_map_idx(id, map_lay[layer])] = sigma_new; + } + """ + ).substitute(), + name="bayesian_inference_kernel", + ) + return bayesian_inference_kernel + + +def class_average_kernel( + width, height, alpha, +): + class_average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U prev_val = map[get_map_idx(id, map_lay[layer])]; + if (prev_val==0){ + U val = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + else{ + U val = ${alpha} *prev_val + (1-${alpha}) * newmap[get_map_idx(id, map_lay[layer])]/(cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + } + """ + ).substitute(alpha=alpha,), + name="class_average_kernel", + ) + return class_average_kernel + + +def add_color_kernel( + width, height, +): + add_color_kernel = cp.ElementwiseKernel( + in_params="raw T p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw V color_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = ( color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid && inside){ + unsigned int color = __float_as_uint(p[id * pcl_channels[0] + pcl_chan[layer]]); + atomicAdd(&color_map[get_map_idx(idx, layer*3)], get_r(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3+1)], get_g(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3 + 2)], get_b(color)); + atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1); + } + """ + ).substitute(width=width), + name="add_color_kernel", + ) + return add_color_kernel + + +def color_average_kernel( + width, height, +): + color_average_kernel = cp.ElementwiseKernel( + in_params="raw V color_map, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = (color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + unsigned int cnt = color_map[get_map_idx(id, pcl_channels[1]*3)]; + if (cnt>0){ + // U prev_color = map[get_map_idx(id, map_lay[layer])]; + unsigned int r = color_map[get_map_idx(id, layer*3)]/(1*cnt); + unsigned int g = color_map[get_map_idx(id, layer*3+1)]/(1*cnt); + unsigned int b = color_map[get_map_idx(id, layer*3+2)]/(1*cnt); + //if (prev_color>=0){ + // unsigned int prev_r = get_r(prev_color); + // unsigned int prev_g = get_g(prev_color); + // unsigned int prev_b = get_b(prev_color); + // unsigned int r = prev_r/2 + color_map[get_map_idx(i, layer*3)]/(2*cnt); + // unsigned int g = prev_g/2 + color_map[get_map_idx(i, layer*3+1)]/(2*cnt); + // unsigned int b = prev_b/2 + color_map[get_map_idx(i, layer*3+2)]/(2*cnt); + //} + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + map[get_map_idx(id, map_lay[layer])] = rgb_; + } + """ + ).substitute(), + name="color_average_kernel", + ) + return color_average_kernel + + + +def map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + util_preamble = string.Template( + """ + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ int get_x_idx(float16 x, float16 center) { + int i = (x - center) / ${resolution} + 0.5 * ${width}; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + int i = (y - center) / ${resolution} + 0.5 * ${height}; + return i; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x == 0 || idx_x == ${width} - 1) { + return false; + } + if (idx_y == 0 || idx_y == ${height} - 1) { + return false; + } + return true; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ float transform_p(float16 x, float16 y, float16 z, + float16 r0, float16 r1, float16 r2, float16 t) { + return r0 * x + r1 * y + r2 * z + t; + } + __device__ float z_noise(float16 z){ + return ${sensor_noise_factor} * z * z; + } + + __device__ float point_sensor_distance(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = (x - sx) * (x - sx) + (y - sy) * (y - sy) + (z - sz) * (z - sz); + return d; + } + + __device__ bool is_valid(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = point_sensor_distance(x, y, z, sx, sy, sz); + float dxy = max(sqrt(x * x + y * y) - ${ramped_height_range_b}, 0.0); + if (d < ${min_valid_distance} * ${min_valid_distance}) { + return false; + } + else if (z - sz > dxy * ${ramped_height_range_a} + ${ramped_height_range_c} || z - sz > ${max_height_range}) { + return false; + } + else { + return true; + } + } + + __device__ float ray_vector(float16 tx, float16 ty, float16 tz, + float16 px, float16 py, float16 pz, + float16& rx, float16& ry, float16& rz){ + float16 vx = px - tx; + float16 vy = py - ty; + float16 vz = pz - tz; + float16 norm = sqrt(vx * vx + vy * vy + vz * vz); + if (norm > 0) { + rx = vx / norm; + ry = vy / norm; + rz = vz / norm; + } + else { + rx = 0; + ry = 0; + rz = 0; + } + return norm; + } + + __device__ float inner_product(float16 x1, float16 y1, float16 z1, + float16 x2, float16 y2, float16 z2) { + + float product = (x1 * x2 + y1 * y2 + z1 * z2); + return product; + } + + """ + ).substitute( + resolution=resolution, + width=width, + height=height, + sensor_noise_factor=sensor_noise_factor, + min_valid_distance=min_valid_distance, + max_height_range=max_height_range, + ramped_height_range_a=ramped_height_range_a, + ramped_height_range_b=ramped_height_range_b, + ramped_height_range_c=ramped_height_range_c, + ) + return util_preamble + + +def add_points_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + wall_num_thresh, + max_ray_length, + cleanup_step, + min_valid_distance, + max_height_range, + cleanup_cos_thresh, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + enable_edge_shaped=True, + enable_visibility_cleanup=True, +): + add_points_kernel = cp.ElementwiseKernel( + in_params="raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map", + out_params="raw U p, raw U map, raw T newmap", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (is_valid(x, y, z, t[0], t[1], t[2])) { + if (is_inside(idx)) { + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U num_points = newmap[get_map_idx(idx, 4)]; + if (abs(map_h - z) > (map_v * ${mahalanobis_thresh})) { + atomicAdd(&map[get_map_idx(idx, 1)], ${outlier_variance}); + } + else { + if (${enable_edge_shaped} && (num_points > ${wall_num_thresh}) && (z < map_h - map_v * ${mahalanobis_thresh} / num_points)) { + // continue; + } + else { + T new_h = (map_h * v + z * map_v) / (map_v + v); + T new_v = (map_v * v) / (map_v + v); + atomicAdd(&newmap[get_map_idx(idx, 0)], new_h); + atomicAdd(&newmap[get_map_idx(idx, 1)], new_v); + atomicAdd(&newmap[get_map_idx(idx, 2)], 1.0); + // is Valid + map[get_map_idx(idx, 2)] = 1; + // Time layer + map[get_map_idx(idx, 4)] = 0.0; + // Upper bound + map[get_map_idx(idx, 5)] = new_h; + map[get_map_idx(idx, 6)] = 0.0; + } + // visibility cleanup + } + } + } + if (${enable_visibility_cleanup}) { + float16 ray_x, ray_y, ray_z; + float16 ray_length = ray_vector(t[0], t[1], t[2], x, y, z, ray_x, ray_y, ray_z); + ray_length = min(ray_length, (float16)${max_ray_length}); + int last_nidx = -1; + for (float16 s=${ray_step}; s < ray_length; s+=${ray_step}) { + // iterate through ray + U nx = t[0] + ray_x * s; + U ny = t[1] + ray_y * s; + U nz = t[2] + ray_z * s; + int nidx = get_idx(nx, ny, center_x[0], center_y[0]); + if (last_nidx == nidx) {continue;} // Skip if we're still in the same cell + else {last_nidx = nidx;} + if (!is_inside(nidx)) {continue;} + + U nmap_h = map[get_map_idx(nidx, 0)]; + U nmap_v = map[get_map_idx(nidx, 1)]; + U nmap_valid = map[get_map_idx(nidx, 2)]; + // traversability + U nmap_trav = map[get_map_idx(nidx, 3)]; + // Time layer + U non_updated_t = map[get_map_idx(nidx, 4)]; + // upper bound + U nmap_upper = map[get_map_idx(nidx, 5)]; + U nmap_is_upper = map[get_map_idx(nidx, 6)]; + + // If point is close or is farther away than ray length, skip. + float16 d = (x - nx) * (x - nx) + (y - ny) * (y - ny) + (z - nz) * (z - nz); + if (d < 0.1 || !is_valid(x, y, z, t[0], t[1], t[2])) {continue;} + + // If invalid, do upper bound check, then skip + if (nmap_valid < 0.5) { + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + continue; + } + // If updated recently, skip + if (non_updated_t < 0.5) {continue;} + + if (nmap_h > nz + 0.01 - min(nmap_v, 1.0) * 0.05) { + // If ray and norm is vertical, skip + U norm_x = norm_map[get_map_idx(nidx, 0)]; + U norm_y = norm_map[get_map_idx(nidx, 1)]; + U norm_z = norm_map[get_map_idx(nidx, 2)]; + float product = inner_product(ray_x, ray_y, ray_z, norm_x, norm_y, norm_z); + if (fabs(product) < ${cleanup_cos_thresh}) {continue;} + U num_points = newmap[get_map_idx(nidx, 3)]; + if (num_points > ${wall_num_thresh} && non_updated_t < 1.0) {continue;} + + // Finally, this cell is penetrated by the ray. + atomicAdd(&map[get_map_idx(nidx, 2)], -${cleanup_step}/(ray_length / ${max_ray_length})); + atomicAdd(&map[get_map_idx(nidx, 1)], ${outlier_variance}); + // Do upper bound check. + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + } + } + } + p[i * 3]= idx; + p[i * 3 + 1] = is_valid(x, y, z, t[0], t[1], t[2]); + p[i * 3 + 2] = is_inside(idx); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + wall_num_thresh=wall_num_thresh, + ray_step=resolution / 2 ** 0.5, + max_ray_length=max_ray_length, + cleanup_step=cleanup_step, + cleanup_cos_thresh=cleanup_cos_thresh, + enable_edge_shaped=int(enable_edge_shaped), + enable_visibility_cleanup=int(enable_visibility_cleanup), + ), + name="add_points_kernel", + ) + return add_points_kernel + + +def error_counting_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + traversability_inlier, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + error_counting_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U p, raw U center_x, raw U center_y, raw U R, raw U t", + out_params="raw U newmap, raw T error, raw T error_cnt", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + // if (!is_valid(z, t[2])) {return;} + if (!is_valid(x, y, z, t[0], t[1], t[2])) {return;} + // if ((x - t[0]) * (x - t[0]) + (y - t[1]) * (y - t[1]) + (z - t[2]) * (z - t[2]) < 0.5) {return;} + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (!is_inside(idx)) { + return; + } + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U map_valid = map[get_map_idx(idx, 2)]; + U map_t = map[get_map_idx(idx, 3)]; + if (map_valid > 0.5 && (abs(map_h - z) < (map_v * ${mahalanobis_thresh})) + && map_v < ${outlier_variance} / 2.0 + && map_t > ${traversability_inlier}) { + T e = z - map_h; + atomicAdd(&error[0], e); + atomicAdd(&error_cnt[0], 1); + atomicAdd(&newmap[get_map_idx(idx, 3)], 1.0); + } + atomicAdd(&newmap[get_map_idx(idx, 4)], 1.0); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + traversability_inlier=traversability_inlier, + ), + name="error_counting_kernel", + ) + return error_counting_kernel + + +def average_map_kernel(width, height, max_variance, initial_variance): + average_map_kernel = cp.ElementwiseKernel( + in_params="raw U newmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U v = map[get_map_idx(i, 1)]; + U valid = map[get_map_idx(i, 2)]; + U new_h = newmap[get_map_idx(i, 0)]; + U new_v = newmap[get_map_idx(i, 1)]; + U new_cnt = newmap[get_map_idx(i, 2)]; + if (new_cnt > 0) { + if (new_v / new_cnt > ${max_variance}) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + else { + map[get_map_idx(i, 0)] = new_h / new_cnt; + map[get_map_idx(i, 1)] = new_v / new_cnt; + map[get_map_idx(i, 2)] = 1; + } + } + if (valid < 0.5) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + """ + ).substitute(max_variance=max_variance, initial_variance=initial_variance), + name="average_map_kernel", + ) + return average_map_kernel + + +def dilation_filter_kernel(width, height, dilation_size): + dilation_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap, raw U newmask", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + newmap[get_map_idx(i, 0)] = h; + if (valid < 0.5) { + U distance = 100; + U near_value = 0; + for (int dy = -${dilation_size}; dy <= ${dilation_size}; dy++) { + for (int dx = -${dilation_size}; dx <= ${dilation_size}; dx++) { + int idx = get_relative_map_idx(i, dx, dy, 0); + if (!is_inside(idx)) {continue;} + U valid = mask[idx]; + if(valid > 0.5 && dx + dy < distance) { + distance = dx + dy; + near_value = map[idx]; + } + } + } + if(distance < 100) { + newmap[get_map_idx(i, 0)] = near_value; + newmask[get_map_idx(i, 0)] = 1.0; + } + } + """ + ).substitute(dilation_size=dilation_size), + name="dilation_filter_kernel", + ) + return dilation_filter_kernel + + +def normal_filter_kernel(width, height, resolution): + normal_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + __device__ float resolution() { + return ${resolution}; + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + if (valid > 0.5) { + int idx_x = get_relative_map_idx(i, 1, 0, 0); + int idx_y = get_relative_map_idx(i, 0, 1, 0); + if (!is_inside(idx_x) || !is_inside(idx_y)) { return; } + float dzdx = (map[idx_x] - h); + float dzdy = (map[idx_y] - h); + float nx = -dzdy / resolution(); + float ny = -dzdx / resolution(); + float nz = 1; + float norm = sqrt((nx * nx) + (ny * ny) + 1); + newmap[get_map_idx(i, 0)] = nx / norm; + newmap[get_map_idx(i, 1)] = ny / norm; + newmap[get_map_idx(i, 2)] = nz / norm; + } + """ + ).substitute(), + name="normal_filter_kernel", + ) + return normal_filter_kernel + + +def polygon_mask_kernel(width, height, resolution): + polygon_mask_kernel = cp.ElementwiseKernel( + in_params="raw U polygon, raw U center_x, raw U center_y, raw int16 polygon_n, raw U polygon_bbox", + out_params="raw U mask", + preamble=string.Template( + """ + __device__ struct Point + { + int x; + int y; + }; + // Given three colinear points p, q, r, the function checks if + // point q lies on line segment 'pr' + __device__ bool onSegment(Point p, Point q, Point r) + { + if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && + q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) + return true; + return false; + } + // To find orientation of ordered triplet (p, q, r). + // The function returns following values + // 0 --> p, q and r are colinear + // 1 --> Clockwise + // 2 --> Counterclockwise + __device__ int orientation(Point p, Point q, Point r) + { + int val = (q.y - p.y) * (r.x - q.x) - + (q.x - p.x) * (r.y - q.y); + if (val == 0) return 0; // colinear + return (val > 0)? 1: 2; // clock or counterclock wise + } + // The function that returns true if line segment 'p1q1' + // and 'p2q2' intersect. + __device__ bool doIntersect(Point p1, Point q1, Point p2, Point q2) + { + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + // General case + if (o1 != o2 && o3 != o4) + return true; + // Special Cases + // p1, q1 and p2 are colinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + // p1, q1 and p2 are colinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + // p2, q2 and p1 are colinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + // p2, q2 and q1 are colinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + return false; // Doesn't fall in any of the above cases + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_idx_x(int idx){ + int idx_x = idx / ${width}; + return idx_x; + } + + __device__ int get_idx_y(int idx){ + int idx_y = idx % ${width}; + return idx_y; + } + + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ float16 round(float16 x) { + return (int)x + (int)(2 * (x - (int)x)); + } + __device__ int get_x_idx(float16 x, float16 center) { + const float resolution = ${resolution}; + const float width = ${width}; + int i = (x - center) / resolution + 0.5 * width; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + const float resolution = ${resolution}; + const float height = ${height}; + int i = (y - center) / resolution + 0.5 * height; + return i; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + // Point p = {get_idx_x(i, center_x[0]), get_idx_y(i, center_y[0])}; + Point p = {get_idx_x(i), get_idx_y(i)}; + Point extreme = {100000, p.y}; + int bbox_min_idx = get_idx(polygon_bbox[0], polygon_bbox[1], center_x[0], center_y[0]); + int bbox_max_idx = get_idx(polygon_bbox[2], polygon_bbox[3], center_x[0], center_y[0]); + Point bmin = {get_idx_x(bbox_min_idx), get_idx_y(bbox_min_idx)}; + Point bmax = {get_idx_x(bbox_max_idx), get_idx_y(bbox_max_idx)}; + if (p.x < bmin.x || p.x > bmax.x || p.y < bmin.y || p.y > bmax.y){ + mask[i] = 0; + return; + } + else { + int intersect_cnt = 0; + for (int j = 0; j < polygon_n[0]; j++) { + Point p1, p2; + int i1 = get_idx(polygon[j * 2 + 0], polygon[j * 2 + 1], center_x[0], center_y[0]); + p1.x = get_idx_x(i1); + p1.y = get_idx_y(i1); + int j2 = (j + 1) % polygon_n[0]; + int i2 = get_idx(polygon[j2 * 2 + 0], polygon[j2 * 2 + 1], center_x[0], center_y[0]); + p2.x = get_idx_x(i2); + p2.y = get_idx_y(i2); + if (doIntersect(p1, p2, p, extreme)) + { + // If the point 'p' is colinear with line segment 'i-next', + // then check if it lies on segment. If it lies, return true, + // otherwise false + if (orientation(p1, p, p2) == 0) { + if (onSegment(p1, p, p2)){ + mask[i] = 1; + return; + } + } + else if(((p1.y <= p.y) && (p2.y > p.y)) || ((p1.y > p.y) && (p2.y <= p.y))){ + intersect_cnt++; + } + } + } + if (intersect_cnt % 2 == 0) { mask[i] = 0; } + else { mask[i] = 1; } + } + """ + ).substitute(a=1), + name="polygon_mask_kernel", + ) + return polygon_mask_kernel + + + +def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_collision): + """ + This function calculates the correspondence between the image and the map. + It takes in the resolution, width, height, and tolerance_z_collision as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _image_to_map_correspondence_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U K, raw U D, raw U image_height, raw U image_width, raw U center", + out_params="raw U uv_correspondence, raw B valid_correspondence", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ bool is_inside_map(int x, int y) { + return (x >= 0 && y >= 0 && x<${width} && y<${height}); + } + __device__ float get_l2_distance(int x0, int y0, int x1, int y1) { + float dx = x0-x1; + float dy = y0-y1; + return sqrt( dx*dx + dy*dy); + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + + // return if gridcell has no valid height + if (map[get_map_idx(i, 2)] != 1){ + return; + } + + // get current cell position + int y0 = i % ${width}; + int x0 = i / ${width}; + + // gridcell 3D point in worldframe TODO reverse x and y + float p1 = (x0-(${width}/2)) * ${resolution} + center[0]; + float p2 = (y0-(${height}/2)) * ${resolution} + center[1]; + float p3 = map[cell_idx] + center[2]; + + // reproject 3D point into image plane + float u = p1 * P[0] + p2 * P[1] + p3 * P[2] + P[3]; + float v = p1 * P[4] + p2 * P[5] + p3 * P[6] + P[7]; + float d = p1 * P[8] + p2 * P[9] + p3 * P[10] + P[11]; + + // filter point behind image plane + if (d <= 0) { + return; + } + u = u/d; + v = v/d; + + // Check if D is all zeros + bool is_D_zero = (D[0] == 0 && D[1] == 0 && D[2] == 0 && D[3] == 0 && D[4] == 0); + + // Apply undistortion using distortion matrix D if not all zeros + if (!is_D_zero) { + float k1 = D[0]; + float k2 = D[1]; + float p1 = D[2]; + float p2 = D[3]; + float k3 = D[4]; + float fx = K[0]; + float fy = K[4]; + float cx = K[2]; + float cy = K[5]; + float x = (u - cx) / fx; + float y = (v - cy) / fy; + float r2 = x * x + y * y; + float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + float u_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float v_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = fx * u_corrected + cx; + v = fy * v_corrected + cy; + } + + // filter point next to image plane + if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ + return; + } + + int y0_c = y0; + int x0_c = x0; + float total_dis = get_l2_distance(x0_c, y0_c, x1,y1); + float z0 = map[cell_idx]; + float delta_z = z1-z0; + + + // bresenham algorithm to iterate over cells in line between camera center and current gridmap cell + // https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + int dx = abs(x1-x0); + int sx = x0 < x1 ? 1 : -1; + int dy = -abs(y1 - y0); + int sy = y0 < y1 ? 1 : -1; + int error = dx + dy; + + bool is_valid = true; + + // iterate over all cells along line + while (1){ + // assumption we do not need to check the height for camera center cell + if (x0 == x1 && y0 == y1){ + break; + } + + // check if height is invalid + if (is_inside_map(x0,y0)){ + int idx = y0 + (x0 * ${width}); + if (map[get_map_idx(idx, 2)]){ + float dis = get_l2_distance(x0_c, y0_c, x0, y0); + float rayheight = z0 + ( dis / total_dis * delta_z); + if ( map[idx] - ${tolerance_z_collision} > rayheight){ + is_valid = false; + break; + } + } + } + + + // computation of next gridcell index in line + int e2 = 2 * error; + if (e2 >= dy){ + if(x0 == x1){ + break; + } + error = error + dy; + x0 = x0 + sx; + } + if (e2 <= dx){ + if (y0 == y1){ + break; + } + error = error + dx; + y0 = y0 + sy; + } + } + + // mark the correspondence + uv_correspondence[get_map_idx(i, 0)] = u; + uv_correspondence[get_map_idx(i, 1)] = v; + valid_correspondence[get_map_idx(i, 0)] = is_valid; + """ + ).substitute(height=height, width=width, resolution=resolution, tolerance_z_collision=tolerance_z_collision), + name="image_to_map_correspondence_kernel", + ) + return _image_to_map_correspondence_kernel + + +def average_correspondences_to_map_kernel(width, height): + """ + This function calculates the average correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _average_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(), + name="average_correspondences_to_map_kernel", + ) + return _average_correspondences_to_map_kernel + + +def exponential_correspondences_to_map_kernel(width, height, alpha): + """ + This function calculates the exponential correspondences to the map. + It takes in the width, height, and alpha as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _exponential_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)] * (1-${alpha}) + ${alpha} * image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(alpha=alpha), + name="exponential_correspondences_to_map_kernel", + ) + return _exponential_correspondences_to_map_kernel + + +def color_correspondences_to_map_kernel(width, height): + """ + This function calculates the color correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _color_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_rgb, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + + int idx_red = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + int idx_green = image_width * image_height + idx_red; + int idx_blue = image_width * image_height * 2 + idx_red; + + unsigned int r = image_rgb[idx_red]; + unsigned int g = image_rgb[idx_green]; + unsigned int b = image_rgb[idx_blue]; + + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + new_sem_map[get_map_idx(i, map_idx)] = rgb_; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + """ + ).substitute(), + name="color_correspondences_to_map_kernel", + ) + return _color_correspondences_to_map_kernel diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/map_initializer.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/map_initializer.py new file mode 100644 index 00000000..0139ea76 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/map_initializer.py @@ -0,0 +1,80 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +from scipy.interpolate import griddata +import numpy as np +import cupy as cp + + +class MapInitializer(object): + def __init__(self, initial_variance, new_variance, xp=np, method="points"): + self.methods = ["points"] + assert method in self.methods, "method should be chosen from {}".format(self.methods) + self.method = method + self.xp = xp + self.initial_variance = initial_variance + self.new_variance = new_variance + + def __call__(self, *args, **kwargs): + if self.method == "points": + self.points_initializer(*args, **kwargs) + else: + return + + def points_initializer(self, elevation_map, points, method="linear"): + """Initialize the map using interpolation between given points + + Args: + elevation_map (cupy._core.core.ndarray): elevation_map data. + points (cupy._core.core.ndarray): points used to interpolate. + method (str): method for interpolation. (nearest, linear, cubic) + + """ + # points from existing map. + points_idx = self.xp.where(elevation_map[2] > 0.5) + values = elevation_map[0, points_idx[0], points_idx[1]] + + # Add external points for interpolation. + points_idx = self.xp.stack(points_idx).T + points_idx = self.xp.vstack([points_idx, points[:, :2]]) + values = self.xp.hstack([values, points[:, 2]]) + + assert points_idx.shape[0] > 3, "Initialization points must be more than 3." + + # Interpolation using griddata function. + w = elevation_map.shape[1] + h = elevation_map.shape[2] + grid_x, grid_y = np.mgrid[0:w, 0:h] + if self.xp == cp: + points_idx = cp.asnumpy(points_idx) + values = cp.asnumpy(values) + interpolated = griddata(points_idx, values, (grid_x, grid_y), method=method) + if self.xp == cp: + interpolated = cp.asarray(interpolated) + + # Update elevation map. + elevation_map[0] = self.xp.nan_to_num(interpolated) + elevation_map[1] = self.xp.where( + self.xp.invert(self.xp.isnan(interpolated)), self.new_variance, self.initial_variance + ) + elevation_map[2] = self.xp.where(self.xp.invert(self.xp.isnan(interpolated)), 1.0, 0.0) + return + + +if __name__ == "__main__": + initializer = MapInitializer(100, 10, method="points", xp=cp) + m = np.zeros((4, 10, 10)) + m[0, 0:5, 2:5] = 0.3 + m[2, 0:5, 2:5] = 1.0 + np.set_printoptions(threshold=100) + print(m[0]) + print(m[1]) + print(m[2]) + points = cp.array([[0, 0, 0.2], [8, 0, 0.2], [6, 9, 0.2]]) + # [3, 3, 0.2]]) + m = cp.asarray(m) + initializer(m, points, method="cubic") + print(m[0]) + print(m[1]) + print(m[2]) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py new file mode 100644 index 00000000..5fc3c110 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py @@ -0,0 +1,300 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +from dataclasses import dataclass, field +import pickle +import numpy as np +from simple_parsing.helpers import Serializable +from dataclasses import field +from typing import Tuple + + +@dataclass +class Parameter(Serializable): + """ + This class holds the parameters for the elevation mapping algorithm. + + Attributes: + resolution: The resolution in meters. + (Default: ``0.04``) + subscriber_cfg: The configuration for the subscriber. + (Default: ``{ "front_cam": { "channels": ["rgb", "person"], "topic_name": "/elevation_mapping/pointcloud_semantic", "data_type": "pointcloud", } }``) + additional_layers: The additional layers for the map. + (Default: ``["color"]``) + fusion_algorithms: The list of fusion algorithms. + (Default: ``[ "image_color", "image_exponential", "pointcloud_average", "pointcloud_bayesian_inference", "pointcloud_class_average", "pointcloud_class_bayesian", "pointcloud_class_max", "pointcloud_color", ]``) + pointcloud_channel_fusions: The fusion for pointcloud channels. + (Default: ``{"rgb": "color", "default": "class_average"}``) + image_channel_fusions: The fusion for image channels. + (Default: ``{"rgb": "color", "default": "exponential"}``) + data_type: The data type for the map. + (Default: ``np.float32``) + average_weight: The weight for the average fusion. + (Default: ``0.5``) + map_length: The map's size in meters. + (Default: ``8.0``) + sensor_noise_factor: The point's noise is sensor_noise_factor*z^2 (z is distance from sensor). + (Default: ``0.05``) + mahalanobis_thresh: Points outside this distance is outlier. + (Default: ``2.0``) + outlier_variance: If point is outlier, add this value to the cell. + (Default: ``0.01``) + drift_compensation_variance_inlier: Cells under this value is used for drift compensation. + (Default: ``0.1``) + time_variance: Add this value when update_variance is called. + (Default: ``0.01``) + time_interval: Time layer is updated with this interval. + (Default: ``0.1``) + max_variance: The maximum variance for each cell. + (Default: ``1.0``) + dilation_size: The dilation filter size before traversability filter. + (Default: ``2``) + dilation_size_initialize: The dilation size after the init. + (Default: ``10``) + drift_compensation_alpha: The drift compensation alpha for smoother update of drift compensation. + (Default: ``1.0``) + traversability_inlier: Cells with higher traversability are used for drift compensation. + (Default: ``0.1``) + wall_num_thresh: If there are more points than this value, only higher points than the current height are used to make the wall more sharp. + (Default: ``100``) + min_height_drift_cnt: Drift compensation only happens if the valid cells are more than this number. + (Default: ``100``) + max_ray_length: The maximum length for ray tracing. + (Default: ``2.0``) + cleanup_step: Substitute this value from validity layer at visibility cleanup. + (Default: ``0.01``) + cleanup_cos_thresh: Substitute this value from validity layer at visibility cleanup. + (Default: ``0.5``) + min_valid_distance: Points with shorter distance will be filtered out. + (Default: ``0.3``) + max_height_range: Points higher than this value from sensor will be filtered out to disable ceiling. + (Default: ``1.0``) + ramped_height_range_a: If z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + (Default: ``0.3``) + ramped_height_range_b: If z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + (Default: ``1.0``) + ramped_height_range_c: If z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + (Default: ``0.2``) + safe_thresh: If traversability is smaller, it is counted as unsafe cell. + (Default: ``0.5``) + safe_min_thresh: Polygon is unsafe if there exists lower traversability than this. + (Default: ``0.5``) + max_unsafe_n: If the number of cells under safe_thresh exceeds this value, polygon is unsafe. + (Default: ``20``) + checker_layer: Layer used for checking safety. + (Default: ``"traversability"``) + min_filter_size: The minimum size for the filter. + (Default: ``5``) + min_filter_iteration: The minimum number of iterations for the filter. + (Default: ``3``) + max_drift: The maximum drift for the compensation. + (Default: ``0.10``) + overlap_clear_range_xy: XY range [m] for clearing overlapped area. This defines the valid area for overlap clearance. (used for multi floor setting) + (Default: ``4.0``) + overlap_clear_range_z: Z range [m] for clearing overlapped area. Cells outside this range will be cleared. (used for multi floor setting) + (Default: ``2.0``) + enable_edge_sharpen: Enable edge sharpening. + (Default: ``True``) + enable_drift_compensation: Enable drift compensation. + (Default: ``True``) + enable_visibility_cleanup: Enable visibility cleanup. + (Default: ``True``) + enable_overlap_clearance: Enable overlap clearance. + (Default: ``True``) + use_only_above_for_upper_bound: Use only above for upper bound. + (Default: ``True``) + use_chainer: Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. Pytorch requires ~2GB more GPU memory compared to chainer but runs faster. + (Default: ``True``) + position_noise_thresh: If the position change is bigger than this value, the drift compensation happens. + (Default: ``0.1``) + orientation_noise_thresh: If the orientation change is bigger than this value, the drift compensation happens. + (Default: ``0.1``) + plugin_config_file: Configuration file for the plugin. + (Default: ``"config/plugin_config.yaml"``) + weight_file: Weight file for traversability filter. + (Default: ``"config/weights.dat"``) + initial_variance: Initial variance for each cell. + (Default: ``10.0``) + initialized_variance: Initialized variance for each cell. + (Default: ``10.0``) + w1: Weights for the first layer. + (Default: ``np.zeros((4, 1, 3, 3))``) + w2: Weights for the second layer. + (Default: ``np.zeros((4, 1, 3, 3))``) + w3: Weights for the third layer. + (Default: ``np.zeros((4, 1, 3, 3))``) + w_out: Weights for the output layer. + (Default: ``np.zeros((1, 12, 1, 1))``) + true_map_length: True length of the map. + (Default: ``None``) + cell_n: Number of cells in the map. + (Default: ``None``) + true_cell_n: True number of cells in the map. + (Default: ``None``) + + """ + resolution: float = 0.04 # resolution in m. + subscriber_cfg: dict = field( + default_factory=lambda: { + "front_cam": { + "channels": ["rgb", "person"], + "topic_name": "/elevation_mapping/pointcloud_semantic", + "data_type": "pointcloud", + } + } + ) # configuration for the subscriber + additional_layers: list = field(default_factory=lambda: ["color"]) # additional layers for the map + fusion_algorithms: list = field( + default_factory=lambda: [ + "image_color", + "image_exponential", + "pointcloud_average", + "pointcloud_bayesian_inference", + "pointcloud_class_average", + "pointcloud_class_bayesian", + "pointcloud_class_max", + "pointcloud_color", + ] + ) # list of fusion algorithms + pointcloud_channel_fusions: dict = field(default_factory=lambda: {"rgb": "color", "default": "class_average"}) # fusion for pointcloud channels + image_channel_fusions: dict = field(default_factory=lambda: {"rgb": "color", "default": "exponential"}) # fusion for image channels + data_type: str = np.float32 # data type for the map + average_weight: float = 0.5 # weight for the average fusion + + map_length: float = 8.0 # map's size in m. + sensor_noise_factor: float = 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). + mahalanobis_thresh: float = 2.0 # points outside this distance is outlier. + outlier_variance: float = 0.01 # if point is outlier, add this value to the cell. + drift_compensation_variance_inlier: float = 0.1 # cells under this value is used for drift compensation. + time_variance: float = 0.01 # add this value when update_variance is called. + time_interval: float = 0.1 # Time layer is updated with this interval. + + max_variance: float = 1.0 # maximum variance for each cell. + dilation_size: float = 2 # dilation filter size before traversability filter. + dilation_size_initialize: float = 10 # dilation size after the init. + drift_compensation_alpha: float = 1.0 # drift compensation alpha for smoother update of drift compensation. + + traversability_inlier: float = 0.1 # cells with higher traversability are used for drift compensation. + wall_num_thresh: float = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. + min_height_drift_cnt: float = 100 # drift compensation only happens if the valid cells are more than this number. + + max_ray_length: float = 2.0 # maximum length for ray tracing. + cleanup_step: float = 0.01 # substitute this value from validity layer at visibility cleanup. + cleanup_cos_thresh: float = 0.5 # substitute this value from validity layer at visibility cleanup. + min_valid_distance: float = 0.3 # points with shorter distance will be filtered out. + max_height_range: float = 1.0 # points higher than this value from sensor will be filtered out to disable ceiling. + ramped_height_range_a: float = 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + ramped_height_range_b: float = 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + ramped_height_range_c: float = 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + + safe_thresh: float = 0.5 # if traversability is smaller, it is counted as unsafe cell. + safe_min_thresh: float = 0.5 # polygon is unsafe if there exists lower traversability than this. + max_unsafe_n: int = 20 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe. + checker_layer: str = "traversability" # layer used for checking safety + + min_filter_size: int = 5 # minimum size for the filter + min_filter_iteration: int = 3 # minimum number of iterations for the filter + + max_drift: float = 0.10 # maximum drift for the compensation + + overlap_clear_range_xy: float = 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) + overlap_clear_range_z: float = 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) + + enable_edge_sharpen: bool = True # enable edge sharpening + enable_drift_compensation: bool = True # enable drift compensation + enable_visibility_cleanup: bool = True # enable visibility cleanup + enable_overlap_clearance: bool = True # enable overlap clearance + use_only_above_for_upper_bound: bool = True # use only above for upper bound + use_chainer: bool = True # use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. + position_noise_thresh: float = 0.1 # if the position change is bigger than this value, the drift compensation happens. + orientation_noise_thresh: float = 0.1 # if the orientation change is bigger than this value, the drift compensation happens. + + plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin + weight_file: str = "config/weights.dat" # weight file for traversability filter + + initial_variance: float = 10.0 # initial variance for each cell. + initialized_variance: float = 10.0 # initialized variance for each cell. + w1: np.ndarray = field(default_factory=lambda: np.zeros((4, 1, 3, 3))) # weights for the first layer + w2: np.ndarray = field(default_factory=lambda: np.zeros((4, 1, 3, 3))) # weights for the second layer + w3: np.ndarray = field(default_factory=lambda: np.zeros((4, 1, 3, 3))) # weights for the third layer + w_out: np.ndarray = field(default_factory=lambda: np.zeros((1, 12, 1, 1))) # weights for the output layer + + # # not configurable params + true_map_length: float = None # true length of the map + cell_n: int = None # number of cells in the map + true_cell_n: int = None # true number of cells in the map + + def load_weights(self, filename): + """ + Load weights from a file into the model's parameters. + + Args: + filename (str): The path to the file containing the weights. + """ + with open(filename, "rb") as file: + weights = pickle.load(file) + self.w1 = weights["conv1.weight"] + self.w2 = weights["conv2.weight"] + self.w3 = weights["conv3.weight"] + self.w_out = weights["conv_final.weight"] + + def get_names(self): + """ + Get the names of the parameters. + + Returns: + list: A list of parameter names. + """ + return list(self.__annotations__.keys()) + + def get_types(self): + """ + Get the types of the parameters. + + Returns: + list: A list of parameter types. + """ + return [v.__name__ for v in self.__annotations__.values()] + + def set_value(self, name, value): + """ + Set the value of a parameter. + + Args: + name (str): The name of the parameter. + value (any): The new value for the parameter. + """ + setattr(self, name, value) + + def get_value(self, name): + """ + Get the value of a parameter. + + Args: + name (str): The name of the parameter. + + Returns: + any: The value of the parameter. + """ + return getattr(self, name) + + def update(self): + """ + Update the parameters related to the map size and resolution. + """ + # +2 is a border for outside map + self.cell_n = int(round(self.map_length / self.resolution)) + 2 + self.true_cell_n = round(self.map_length / self.resolution) + self.true_map_length = self.true_cell_n * self.resolution + + +if __name__ == "__main__": + param = Parameter() + print(param) + print(param.resolution) + param.set_value("resolution", 0.1) + print(param.resolution) + + print("names ", param.get_names()) + print("types ", param.get_types()) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/__init__.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/erosion.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/erosion.py new file mode 100644 index 00000000..7b0211df --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/erosion.py @@ -0,0 +1,113 @@ +# +# Copyright (c) 2024, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cv2 as cv +import cupy as cp +import numpy as np + +from typing import List + +from .plugin_manager import PluginBase + + +class Erosion(PluginBase): + """ + This class is used for applying erosion to an elevation map or specific layers within it. + Erosion is a morphological operation that is used to remove small-scale details from a binary image. + + Args: + kernel_size (int): Size of the erosion kernel. Default is 3, which means a 3x3 square kernel. + iterations (int): Number of times erosion is applied. Default is 1. + **kwargs (): Additional keyword arguments. + """ + + def __init__( + self, + input_layer_name="traversability", + kernel_size: int = 3, + iterations: int = 1, + reverse: bool = False, + default_layer_name: str = "traversability", + **kwargs, + ): + super().__init__() + self.input_layer_name = input_layer_name + self.kernel_size = kernel_size + self.iterations = iterations + self.reverse = reverse + self.default_layer_name = default_layer_name + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + Applies erosion to the given elevation map. + + Args: + elevation_map (cupy._core.core.ndarray): The elevation map to be eroded. + layer_names (List[str]): Names of the layers in the elevation map. + plugin_layers (cupy._core.core.ndarray): Layers provided by other plugins. + plugin_layer_names (List[str]): Names of the layers provided by other plugins. + *args (): Additional arguments. + + Returns: + cupy._core.core.ndarray: The eroded elevation map. + """ + # Convert the elevation map to a format suitable for erosion (if necessary) + layer_data = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + self.input_layer_name, + ) + if layer_data is None: + print(f"No layers are found, using {self.default_layer_name}!") + layer_data = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + self.default_layer_name, + ) + if layer_data is None: + print(f"No layers are found, using traversability!") + layer_data = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + "traversability", + ) + layer_np = cp.asnumpy(layer_data) + + # Define the erosion kernel + kernel = np.ones((self.kernel_size, self.kernel_size), np.uint8) + + if self.reverse: + layer_np = 1 - layer_np + # Apply erosion + layer_min = float(layer_np.min()) + layer_max = float(layer_np.max()) + layer_np_normalized = ((layer_np - layer_min) * 255 / (layer_max - layer_min)).astype("uint8") + eroded_map_np = cv.erode(layer_np_normalized, kernel, iterations=self.iterations) + eroded_map_np = eroded_map_np.astype(np.float32) * (layer_max - layer_min) / 255 + layer_min + if self.reverse: + eroded_map_np = 1 - eroded_map_np + + # Convert back to cupy array and return + return cp.asarray(eroded_map_np) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/features_pca.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/features_pca.py new file mode 100644 index 00000000..94f88823 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/features_pca.py @@ -0,0 +1,96 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +from typing import List +import re + +from elevation_mapping_cupy.plugins.plugin_manager import PluginBase +from sklearn.decomposition import PCA + + +class FeaturesPca(PluginBase): + """This is a filter to create a pca layer of the semantic features in the map. + + Args: + cell_n (int): width and height of the elevation map. + classes (ruamel.yaml.comments.CommentedSeq): + **kwargs (): + """ + + def __init__( + self, cell_n: int = 100, process_layer_names: List[str] = [], **kwargs, + ): + super().__init__() + self.process_layer_names = process_layer_names + + def get_layer_indices(self, layer_names: List[str]) -> List[int]: + """ Get the indices of the layers that are to be processed using regular expressions. + Args: + layer_names (List[str]): List of layer names. + Returns: + List[int]: List of layer indices. + """ + indices = [] + for i, layer_name in enumerate(layer_names): + if any(re.match(pattern, layer_name) for pattern in self.process_layer_names): + indices.append(i) + return indices + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + # get indices of all layers that contain semantic features information + data = [] + for m, layer_names in zip( + [elevation_map, plugin_layers, semantic_map], [layer_names, plugin_layer_names, semantic_layer_names] + ): + layer_indices = self.get_layer_indices(layer_names) + if len(layer_indices) > 0: + n_c = m[layer_indices].shape[1] + data_i = cp.reshape(m[layer_indices], (len(layer_indices), -1)).T.get() + data_i = np.clip(data_i, -1, 1) + data.append(data_i) + if len(data) > 0: + data = np.concatenate(data, axis=1) + # check which has the highest value + n_components = 3 + pca = PCA(n_components=n_components).fit(data) + pca_descriptors = pca.transform(data) + img_pca = pca_descriptors.reshape(n_c, n_c, n_components) + comp = img_pca # [:, :, -3:] + comp_min = comp.min(axis=(0, 1)) + comp_max = comp.max(axis=(0, 1)) + if (comp_max - comp_min).any() != 0: + comp_img = np.divide((comp - comp_min), (comp_max - comp_min)) + pca_map = (comp_img * 255).astype(np.uint8) + r = np.asarray(pca_map[:, :, 0], dtype=np.uint32) + g = np.asarray(pca_map[:, :, 1], dtype=np.uint32) + b = np.asarray(pca_map[:, :, 2], dtype=np.uint32) + rgb_arr = np.array((r << 16) | (g << 8) | (b << 0), dtype=np.uint32) + rgb_arr.dtype = np.float32 + return cp.asarray(rgb_arr) + else: + return cp.zeros_like(elevation_map[0]) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/inpainting.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/inpainting.py new file mode 100644 index 00000000..3e926b00 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/inpainting.py @@ -0,0 +1,63 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +from typing import List +import cupyx.scipy.ndimage as ndimage +import numpy as np +import cv2 as cv + +from .plugin_manager import PluginBase + + +class Inpainting(PluginBase): + """ + This class is used for inpainting, a process of reconstructing lost or deteriorated parts of images and videos. + + Args: + cell_n (int): The number of cells. Default is 100. + method (str): The inpainting method. Options are 'telea' or 'ns' (Navier-Stokes). Default is 'telea'. + **kwargs (): Additional keyword arguments. + """ + + def __init__(self, cell_n: int = 100, method: str = "telea", **kwargs): + super().__init__() + if method == "telea": + self.method = cv.INPAINT_TELEA + elif method == "ns": # Navier-Stokes + self.method = cv.INPAINT_NS + else: # default method + self.method = cv.INPAINT_TELEA + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + mask = cp.asnumpy((elevation_map[2] < 0.5).astype("uint8")) + if (mask < 1).any(): + h = elevation_map[0] + h_max = float(h[mask < 1].max()) + h_min = float(h[mask < 1].min()) + h = cp.asnumpy((elevation_map[0] - h_min) * 255 / (h_max - h_min)).astype("uint8") + dst = np.array(cv.inpaint(h, mask, 1, self.method)) + h_inpainted = dst.astype(np.float32) * (h_max - h_min) / 255 + h_min + return cp.asarray(h_inpainted).astype(np.float64) + else: + return elevation_map[0] diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/max_layer_filter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/max_layer_filter.py new file mode 100644 index 00000000..33bc069a --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/max_layer_filter.py @@ -0,0 +1,108 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +from typing import List + +from elevation_mapping_cupy.plugins.plugin_manager import PluginBase + + +class MaxLayerFilter(PluginBase): + """Applies a maximum filter to the input layers and updates the traversability map. + This can be used to enhance navigation by identifying traversable areas. + + Args: + cell_n (int): The width and height of the elevation map. + reverse (list): A list of boolean values indicating whether to reverse the filter operation for each layer. Default is [True]. + min_or_max (str): A string indicating whether to apply a minimum or maximum filter. Accepts "min" or "max". Default is "max". + layers (list): List of layers for semantic traversability. Default is ["traversability"]. + thresholds (list): List of thresholds for each layer. If the value is bigger than a threshold, assign 1.0 otherwise 0.0. If it is False, it does not apply. Default is [False]. + **kwargs: Additional keyword arguments. + """ + + def __init__( + self, + cell_n: int = 100, + layers: list = ["traversability"], + reverse: list = [True], + min_or_max: str = "max", + thresholds: list = [False], + scales: list = [1.0], + default_value: float = 0.0, + **kwargs, + ): + super().__init__() + self.layers = layers + self.reverse = reverse + self.min_or_max = min_or_max + self.thresholds = thresholds + self.scales = scales + self.default_value = default_value + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + layers = [] + for it, name in enumerate(self.layers): + layer = self.get_layer_data( + elevation_map, layer_names, plugin_layers, plugin_layer_names, semantic_map, semantic_layer_names, name + ) + if layer is None: + continue + if isinstance(self.default_value, float): + layer = cp.where(layer == 0.0, float(self.default_value), layer) + elif isinstance(self.default_value, str): + default_layer = self.get_layer_data( + elevation_map, + layer_names, + plugin_layers, + plugin_layer_names, + semantic_map, + semantic_layer_names, + self.default_value, + ) + layer = cp.where(layer == 0, default_layer, layer) + if self.reverse[it]: + layer = 1.0 - layer + if len(self.scales) > it and isinstance(self.scales[it], float): + layer = layer * float(self.scales[it]) + if isinstance(self.thresholds[it], float): + layer = cp.where(layer > float(self.thresholds[it]), 1, 0) + layers.append(layer) + if len(layers) == 0: + print("No layers are found, returning traversability!") + if isinstance(self.default_value, float): + layer = cp.ones_like(elevation_map[0]) + layer *= float(self.default_value) + return layer + else: + idx = layer_names.index("traversability") + return elevation_map[idx] + result = cp.stack(layers, axis=0) + if self.min_or_max == "min": + result = cp.min(result, axis=0) + else: + result = cp.max(result, axis=0) + return result diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/min_filter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/min_filter.py new file mode 100644 index 00000000..ebf4300a --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/min_filter.py @@ -0,0 +1,118 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string +from typing import List + +from .plugin_manager import PluginBase + + +class MinFilter(PluginBase): + """This is a filter to fill in invalid cells with minimum values around. + + Args: + cell_n (int): width of the elevation map. + dilation_size (int): The size of the patch to search for minimum value for each iteration. + iteration_n (int): The number of iteration to repeat the same filter. + **kwargs (): + """ + + def __init__(self, cell_n: int = 100, dilation_size: int = 5, iteration_n: int = 5, **kwargs): + super().__init__() + self.iteration_n = iteration_n + self.width = cell_n + self.height = cell_n + self.min_filtered = cp.zeros((self.width, self.height)) + self.min_filtered_mask = cp.zeros((self.width, self.height)) + self.min_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap, raw U newmask", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + """ + ).substitute(width=self.width, height=self.height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + if (valid < 0.5) { + U min_value = 1000000.0; + for (int dy = -${dilation_size}; dy <= ${dilation_size}; dy++) { + for (int dx = -${dilation_size}; dx <= ${dilation_size}; dx++) { + int idx = get_relative_map_idx(i, dx, dy, 0); + if (!is_inside(idx)) {continue;} + U valid = newmask[idx]; + U value = newmap[idx]; + if(valid > 0.5 && value < min_value) { + min_value = value; + } + } + } + if (min_value < 1000000 - 1) { + newmap[get_map_idx(i, 0)] = min_value; + newmask[get_map_idx(i, 0)] = 0.6; + } + } + """ + ).substitute(dilation_size=dilation_size), + name="min_filter_kernel", + ) + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + self.min_filtered = elevation_map[0].copy() + self.min_filtered_mask = elevation_map[2].copy() + for i in range(self.iteration_n): + self.min_filter_kernel( + elevation_map[0], + elevation_map[2], + self.min_filtered, + self.min_filtered_mask, + size=(self.width * self.height), + ) + # If there's no more mask, break + if (self.min_filtered_mask > 0.5).all(): + break + min_filtered = cp.where(self.min_filtered_mask > 0.5, self.min_filtered.copy(), cp.nan) + return min_filtered diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/plugin_manager.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/plugin_manager.py new file mode 100644 index 00000000..26f81d21 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/plugin_manager.py @@ -0,0 +1,268 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +from abc import ABC +import cupy as cp +from typing import List, Dict, Optional +import importlib +import inspect +from dataclasses import dataclass +from ruamel.yaml import YAML +from inspect import signature + + +@dataclass +class PluginParams: + name: str + layer_name: str + fill_nan: bool = False # fill nan to invalid region + is_height_layer: bool = False # if this is a height layer + + +class PluginBase(ABC): + """ + This is a base class of Plugins + """ + + def __init__(self, *args, **kwargs): + """ + + Args: + plugin_params : PluginParams + The parameter of callback + """ + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + **kwargs, + ) -> cp.ndarray: + """This gets the elevation map data and plugin layers as a cupy array. + + + Args: + elevation_map (): + layer_names (): + plugin_layers (): + plugin_layer_names (): + semantic_map (): + semantic_layer_names (): + + Run your processing here and return the result. + layer of elevation_map 0: elevation + 1: variance + 2: is_valid + 3: traversability + 4: time + 5: upper_bound + 6: is_upper_bound + You can also access to the other plugins' layer with plugin_layers and plugin_layer_names + + """ + pass + + def get_layer_data( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + name: str, + ) -> Optional[cp.ndarray]: + """ + Retrieve a copy of the layer data from the elevation, plugin, or semantic maps based on the layer name. + + Args: + elevation_map (cp.ndarray): The elevation map containing various layers. + layer_names (List[str]): A list of names for each layer in the elevation map. + plugin_layers (cp.ndarray): The plugin layers containing additional data. + plugin_layer_names (List[str]): A list of names for each layer in the plugin layers. + semantic_map (cp.ndarray): The semantic map containing various layers. + semantic_layer_names (List[str]): A list of names for each layer in the semantic map. + name (str): The name of the layer to retrieve. + + Returns: + Optional[cp.ndarray]: A copy of the requested layer as a cupy ndarray if found, otherwise None. + """ + if name in layer_names: + idx = layer_names.index(name) + layer = elevation_map[idx].copy() + elif name in plugin_layer_names: + idx = plugin_layer_names.index(name) + layer = plugin_layers[idx].copy() + elif name in semantic_layer_names: + idx = semantic_layer_names.index(name) + layer = semantic_map[idx].copy() + else: + print(f"Could not find layer {name}!") + layer = None + return layer + + +class PluginManager(object): + """ + This manages the plugins. + """ + + def __init__(self, cell_n: int): + self.cell_n = cell_n + + def init(self, plugin_params: List[PluginParams], extra_params: List[Dict]): + self.plugin_params = plugin_params + + self.plugins = [] + for param, extra_param in zip(plugin_params, extra_params): + m = importlib.import_module("." + param.name, package="elevation_mapping_cupy.plugins") # -> 'module' + for name, obj in inspect.getmembers(m): + if inspect.isclass(obj) and issubclass(obj, PluginBase) and name != "PluginBase": + # Add cell_n to params + extra_param["cell_n"] = self.cell_n + self.plugins.append(obj(**extra_param)) + self.layers = cp.zeros((len(self.plugins), self.cell_n, self.cell_n), dtype=cp.float32) + self.layer_names = self.get_layer_names() + self.plugin_names = self.get_plugin_names() + + def load_plugin_settings(self, file_path: str): + print("Start loading plugins...") + cfg = YAML().load(open(file_path, "r")) + plugin_params = [] + extra_params = [] + for k, v in cfg.items(): + if v["enable"]: + plugin_params.append( + PluginParams( + name=k if not "type" in v else v["type"], + layer_name=v["layer_name"], + fill_nan=v["fill_nan"], + is_height_layer=v["is_height_layer"], + ) + ) + extra_params.append(v["extra_params"]) + self.init(plugin_params, extra_params) + print("Loaded plugins are ", *self.plugin_names) + + def get_layer_names(self): + names = [] + for obj in self.plugin_params: + names.append(obj.layer_name) + return names + + def get_plugin_names(self): + names = [] + for obj in self.plugin_params: + names.append(obj.name) + return names + + def get_plugin_index_with_name(self, name: str) -> int: + try: + idx = self.plugin_names.index(name) + return idx + except Exception as e: + print("Error with plugin {}: {}".format(name, e)) + return None + + def get_layer_index_with_name(self, name: str) -> int: + try: + idx = self.layer_names.index(name) + return idx + except Exception as e: + print("Error with layer {}: {}".format(name, e)) + return None + + def update_with_name( + self, + name: str, + elevation_map: cp.ndarray, + layer_names: List[str], + semantic_map=None, + semantic_params=None, + rotation=None, + elements_to_shift={}, + ): + idx = self.get_layer_index_with_name(name) + if idx is not None and idx < len(self.plugins): + n_param = len(signature(self.plugins[idx]).parameters) + if n_param == 5: + self.layers[idx] = self.plugins[idx](elevation_map, layer_names, self.layers, self.layer_names) + elif n_param == 7: + self.layers[idx] = self.plugins[idx]( + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + ) + elif n_param == 8: + self.layers[idx] = self.plugins[idx]( + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + rotation, + ) + else: + self.layers[idx] = self.plugins[idx]( + elevation_map, + layer_names, + self.layers, + self.layer_names, + semantic_map, + semantic_params, + rotation, + elements_to_shift, + ) + + def get_map_with_name(self, name: str) -> cp.ndarray: + idx = self.get_layer_index_with_name(name) + if idx is not None: + return self.layers[idx] + + def get_param_with_name(self, name: str) -> PluginParams: + idx = self.get_layer_index_with_name(name) + if idx is not None: + return self.plugin_params[idx] + + +if __name__ == "__main__": + plugins = [ + PluginParams(name="min_filter", layer_name="min_filter"), + PluginParams(name="smooth_filter", layer_name="smooth"), + ] + extra_params = [ + {"dilation_size": 5, "iteration_n": 5}, + {"input_layer_name": "elevation2"}, + ] + manager = PluginManager(200) + manager.load_plugin_settings("../config/plugin_config.yaml") + print(manager.layer_names) + print(manager.plugin_names) + elevation_map = cp.zeros((7, 200, 200)).astype(cp.float32) + layer_names = [ + "elevation", + "variance", + "is_valid", + "traversability", + "time", + "upper_bound", + "is_upper_bound", + ] + elevation_map[0] = cp.random.randn(200, 200) + elevation_map[2] = cp.abs(cp.random.randn(200, 200)) + print("map", elevation_map[0]) + print("layer map ", manager.layers) + manager.update_with_name("min_filter", elevation_map, layer_names) + manager.update_with_name("smooth_filter", elevation_map, layer_names) + # manager.update_with_name("sem_fil", elevation_map, layer_names, semantic_map=semantic_map) + print(manager.get_map_with_name("smooth")) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/robot_centric_elevation.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/robot_centric_elevation.py new file mode 100644 index 00000000..80ac73a3 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/robot_centric_elevation.py @@ -0,0 +1,121 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string +from typing import List +from .plugin_manager import PluginBase + + +class RobotCentricElevation(PluginBase): + """Generates an elevation map with respect to the robot frame. + + Args: + cell_n (int): + resolution (ruamel.yaml.scalarfloat.ScalarFloat): + threshold (ruamel.yaml.scalarfloat.ScalarFloat): + use_threshold (bool): + **kwargs (): + """ + + def __init__( + self, cell_n: int = 100, resolution: float = 0.05, threshold: float = 0.4, use_threshold: bool = 0, **kwargs + ): + super().__init__() + self.width = cell_n + self.height = cell_n + self.min_filtered = cp.zeros((self.width, self.height), dtype=cp.float32) + + self.base_elevation_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask, raw U R", + out_params="raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + __device__ float get_map_x(int idx){ + float idx_x = idx / ${width}* ${resolution}; + return idx_x; + } + __device__ float get_map_y(int idx){ + float idx_y = idx % ${width}* ${resolution}; + return idx_y; + } + __device__ float transform_p(float x, float y, float z, + float r0, float r1, float r2) { + return r0 * x + r1 * y + r2 * z ; + } + """ + ).substitute(width=self.width, height=self.height, resolution=resolution), + operation=string.Template( + """ + U rz = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + if (valid > 0.5) { + U rx = get_map_x(get_map_idx(i, 0)); + U ry = get_map_y(get_map_idx(i, 0)); + U x_b = transform_p(rx, ry, rz, R[0], R[1], R[2]); + U y_b = transform_p(rx, ry, rz, R[3], R[4], R[5]); + U z_b = transform_p(rx, ry, rz, R[6], R[7], R[8]); + if (${use_threshold} && z_b>= ${threshold} ) { + newmap[get_map_idx(i, 0)] = 1.0; + } + else if (${use_threshold} && z_b< ${threshold} ){ + newmap[get_map_idx(i, 0)] = 0.0; + } + else{ + newmap[get_map_idx(i, 0)] = z_b; + } + } + """ + ).substitute(threshold=threshold, use_threshold=int(use_threshold)), + name="base_elevation_kernel", + ) + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + rotation, + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): + rotation (cupy._core.core.ndarray): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + # Process maps here + # check that transform is a ndarray + self.min_filtered = elevation_map[0].copy() + self.base_elevation_kernel( + elevation_map[0], elevation_map[2], rotation, self.min_filtered, size=(self.width * self.height), + ) + return self.min_filtered diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_filter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_filter.py new file mode 100644 index 00000000..0ec56010 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_filter.py @@ -0,0 +1,133 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +from typing import List +import re + +from elevation_mapping_cupy.plugins.plugin_manager import PluginBase + + +class SemanticFilter(PluginBase): + """This is a filter to create a one hot encoded map of the class probabilities. + + Args: + cell_n (int): width and height of the elevation map. + classes (list): List of classes for semantic filtering. Default is ["person", "grass"]. + **kwargs: Additional keyword arguments. + """ + + def __init__( + self, cell_n: int = 100, classes: list = ["person", "grass"], **kwargs, + ): + super().__init__() + self.indices = [] + self.classes = classes + self.color_encoding = self.transform_color() + + def color_map(self, N: int = 256, normalized: bool = False): + """ + Creates a color map with N different colors. + + Args: + N (int, optional): The number of colors in the map. Defaults to 256. + normalized (bool, optional): If True, the colors are normalized to the range [0,1]. Defaults to False. + + Returns: + np.ndarray: The color map. + """ + + def bitget(byteval, idx): + return (byteval & (1 << idx)) != 0 + + dtype = "float32" if normalized else "uint8" + cmap = np.zeros((N + 1, 3), dtype=dtype) + for i in range(N + 1): + r = g = b = 0 + c = i + for j in range(8): + r = r | (bitget(c, 0) << 7 - j) + g = g | (bitget(c, 1) << 7 - j) + b = b | (bitget(c, 2) << 7 - j) + c = c >> 3 + + cmap[i] = np.array([r, g, b]) + + cmap = cmap / 255 if normalized else cmap + cmap[1] = np.array([81, 113, 162]) + cmap[2] = np.array([81, 113, 162]) + cmap[3] = np.array([188, 63, 59]) + return cmap[1:] + + def transform_color(self): + """ + Transforms the color map into a format that can be used for semantic filtering. + + Returns: + cp.ndarray: The transformed color map. + """ + color_classes = self.color_map(255) + r = np.asarray(color_classes[:, 0], dtype=np.uint32) + g = np.asarray(color_classes[:, 1], dtype=np.uint32) + b = np.asarray(color_classes[:, 2], dtype=np.uint32) + rgb_arr = np.array((r << 16) | (g << 8) | (b << 0), dtype=np.uint32) + rgb_arr.dtype = np.float32 + return cp.asarray(rgb_arr) + + def get_layer_indices(self, layer_names: List[str]) -> List[int]: + """ Get the indices of the layers that are to be processed using regular expressions. + Args: + layer_names (List[str]): List of layer names. + Returns: + List[int]: List of layer indices. + """ + indices = [] + for i, layer_name in enumerate(layer_names): + if any(re.match(pattern, layer_name) for pattern in self.classes): + indices.append(i) + return indices + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + rotation, + elements_to_shift, + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + # get indices of all layers that contain semantic class information + data = [] + for m, layer_names in zip( + [elevation_map, plugin_layers, semantic_map], [layer_names, plugin_layer_names, semantic_layer_names] + ): + layer_indices = self.get_layer_indices(layer_names) + if len(layer_indices) > 0: + data.append(m[layer_indices]) + if len(data) > 0: + data = cp.concatenate(data, axis=0) + class_map = cp.amax(data, axis=0) + class_map_id = cp.argmax(data, axis=0) + else: + class_map = cp.zeros_like(elevation_map[0]) + class_map_id = cp.zeros_like(elevation_map[0], dtype=cp.int32) + enc = self.color_encoding[class_map_id] + return enc diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_traversability.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_traversability.py new file mode 100644 index 00000000..5f6ef1d5 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/semantic_traversability.py @@ -0,0 +1,83 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import numpy as np +from typing import List + +from elevation_mapping_cupy.plugins.plugin_manager import PluginBase + + +class SemanticTraversability(PluginBase): + """Extracts traversability and elevations from layers and generates an updated traversability that can be used by checker. + + Args: + cell_n (int): The width and height of the elevation map. + layers (list): List of layers for semantic traversability. Default is ["traversability"]. + thresholds (list): List of thresholds for each layer. Default is [0.5]. + type (list): List of types for each layer. Default is ["traversability"]. + **kwargs: Additional keyword arguments. + """ + + def __init__( + self, + cell_n: int = 100, + layers: list = ["traversability"], + thresholds: list = [0.5], + type: list = ["traversability"], + **kwargs, + ): + super().__init__() + self.layers = layers + self.thresholds = cp.asarray(thresholds) + self.type = type + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + semantic_map: cp.ndarray, + semantic_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + semantic_map (elevation_mapping_cupy.semantic_map.SemanticMap): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + # get indices of all layers that + map = cp.zeros(elevation_map[2].shape, np.float32) + tempo = cp.zeros(elevation_map[2].shape, np.float32) + for it, name in enumerate(self.layers): + if name in layer_names: + idx = layer_names.index(name) + tempo = elevation_map[idx] + # elif name in semantic_params.additional_layers: + # idx = semantic_params.additional_layers.index(name) + # tempo = semantic_map[idx] + elif name in plugin_layer_names: + idx = plugin_layer_names.index(name) + tempo = plugin_layers[idx] + else: + print("Layer {} is not in the map, returning traversabiltiy!".format(name)) + return + if self.type[it] == "traversability": + tempo = cp.where(tempo <= self.thresholds[it], 1, 0) + map += tempo + else: + tempo = cp.where(tempo >= self.thresholds[it], 1, 0) + map += tempo + map = cp.where(map <= 0.9, 0.1, 1) + + return map diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/smooth_filter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/smooth_filter.py new file mode 100644 index 00000000..ae48b688 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/plugins/smooth_filter.py @@ -0,0 +1,59 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +from typing import List +import cupyx.scipy.ndimage as ndimage + +from .plugin_manager import PluginBase + + +class SmoothFilter(PluginBase): + """ + SmoothFilter is a class that applies a smoothing filter + to the elevation map. The filter is applied to the layer specified by the input_layer_name parameter. + If the specified layer is not found, the filter is applied to the elevation layer. + + Args: + cell_n (int): The width and height of the elevation map. Default is 100. + input_layer_name (str): The name of the layer to which the filter should be applied. Default is "elevation". + **kwargs: Additional keyword arguments. + """ + + def __init__(self, cell_n: int = 100, input_layer_name: str = "elevation", **kwargs): + super().__init__() + self.input_layer_name = input_layer_name + + def __call__( + self, + elevation_map: cp.ndarray, + layer_names: List[str], + plugin_layers: cp.ndarray, + plugin_layer_names: List[str], + *args, + ) -> cp.ndarray: + """ + + Args: + elevation_map (cupy._core.core.ndarray): + layer_names (List[str]): + plugin_layers (cupy._core.core.ndarray): + plugin_layer_names (List[str]): + *args (): + + Returns: + cupy._core.core.ndarray: + """ + if self.input_layer_name in layer_names: + idx = layer_names.index(self.input_layer_name) + h = elevation_map[idx] + elif self.input_layer_name in plugin_layer_names: + idx = plugin_layer_names.index(self.input_layer_name) + h = plugin_layers[idx] + else: + print("layer name {} was not found. Using elevation layer.".format(self.input_layer_name)) + h = elevation_map[0] + hs1 = ndimage.uniform_filter(h, size=3) + hs1 = ndimage.uniform_filter(hs1, size=3) + return hs1 diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/semantic_map.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/semantic_map.py new file mode 100644 index 00000000..a06b7361 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/semantic_map.py @@ -0,0 +1,400 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +from elevation_mapping_cupy.parameter import Parameter +import cupy as cp +import numpy as np +from typing import List, Dict +import re + + +from elevation_mapping_cupy.fusion.fusion_manager import FusionManager + +xp = cp + + +class SemanticMap: + def __init__(self, param: Parameter): + """ + + Args: + param (elevation_mapping_cupy.parameter.Parameter): + """ + + self.param = param + + self.layer_specs_points = {} + self.layer_specs_image = {} + self.layer_names = [] + self.unique_fusion = [] + self.unique_data = [] + self.elements_to_shift = {} + + self.unique_fusion = self.param.fusion_algorithms + + self.amount_layer_names = len(self.layer_names) + + self.semantic_map = xp.zeros( + (self.amount_layer_names, self.param.cell_n, self.param.cell_n), dtype=param.data_type, + ) + self.new_map = xp.zeros((self.amount_layer_names, self.param.cell_n, self.param.cell_n), param.data_type,) + # which layers should be reset to zero at each update, per default everyone, + # if a layer should not be reset, it is defined in compile_kernels function + self.delete_new_layers = cp.ones(self.new_map.shape[0], cp.bool8) + self.fusion_manager = FusionManager(self.param) + + def clear(self): + """Clear the semantic map.""" + self.semantic_map *= 0.0 + + def initialize_fusion(self): + """Initialize the fusion algorithms.""" + for fusion in self.unique_fusion: + if "pointcloud_class_bayesian" == fusion: + pcl_ids = self.get_layer_indices("class_bayesian", self.layer_specs_points) + self.delete_new_layers[pcl_ids] = 0 + if "pointcloud_class_max" == fusion: + pcl_ids = self.get_layer_indices("class_max", self.layer_specs_points) + self.delete_new_layers[pcl_ids] = 0 + layer_cnt = self.param.fusion_algorithms.count("class_max") + id_max = cp.zeros((layer_cnt, self.param.cell_n, self.param.cell_n), dtype=cp.uint32,) + self.elements_to_shift["id_max"] = id_max + self.fusion_manager.register_plugin(fusion) + + def update_fusion_setting(self): + """ + Update the fusion settings. + """ + for fusion in self.unique_fusion: + if "pointcloud_class_bayesian" == fusion: + pcl_ids = self.get_layer_indices("class_bayesian", self.layer_specs_points) + self.delete_new_layers[pcl_ids] = 0 + if "pointcloud_class_max" == fusion: + pcl_ids = self.get_layer_indices("class_max", self.layer_specs_points) + self.delete_new_layers[pcl_ids] = 0 + layer_cnt = self.param.fusion_algorithms.count("class_max") + id_max = cp.zeros((layer_cnt, self.param.cell_n, self.param.cell_n), dtype=cp.uint32,) + self.elements_to_shift["id_max"] = id_max + + def add_layer(self, name): + """ + Add a new layer to the semantic map. + + Args: + name (str): The name of the new layer. + """ + if name not in self.layer_names: + self.layer_names.append(name) + self.semantic_map = cp.append( + self.semantic_map, + cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type), + axis=0, + ) + self.new_map = cp.append( + self.new_map, cp.zeros((1, self.param.cell_n, self.param.cell_n), dtype=self.param.data_type), axis=0, + ) + self.delete_new_layers = cp.append(self.delete_new_layers, cp.array([1], dtype=cp.bool8)) + + def pad_value(self, x, shift_value, idx=None, value=0.0): + """Create a padding of the map along x,y-axis according to amount that has shifted. + + Args: + x (cupy._core.core.ndarray): + shift_value (cupy._core.core.ndarray): + idx (Union[None, int, None, None]): + value (float): + """ + if idx is None: + if shift_value[0] > 0: + x[:, : shift_value[0], :] = value + elif shift_value[0] < 0: + x[:, shift_value[0] :, :] = value + if shift_value[1] > 0: + x[:, :, : shift_value[1]] = value + elif shift_value[1] < 0: + x[:, :, shift_value[1] :] = value + else: + if shift_value[0] > 0: + x[idx, : shift_value[0], :] = value + elif shift_value[0] < 0: + x[idx, shift_value[0] :, :] = value + if shift_value[1] > 0: + x[idx, :, : shift_value[1]] = value + elif shift_value[1] < 0: + x[idx, :, shift_value[1] :] = value + + def shift_map_xy(self, shift_value): + """Shift the map along x,y-axis according to shift values. + + Args: + shift_value: + """ + self.semantic_map = cp.roll(self.semantic_map, shift_value, axis=(1, 2)) + self.pad_value(self.semantic_map, shift_value, value=0.0) + self.new_map = cp.roll(self.new_map, shift_value, axis=(1, 2)) + self.pad_value(self.new_map, shift_value, value=0.0) + for el in self.elements_to_shift.values(): + el = cp.roll(el, shift_value, axis=(1, 2)) + self.pad_value(el, shift_value, value=0.0) + + def get_fusion( + self, channels: List[str], channel_fusions: Dict[str, str], layer_specs: Dict[str, str] + ) -> List[str]: + """Get all fusion algorithms that need to be applied to a specific pointcloud. + + Args: + channels (List[str]): + """ + fusion_list = [] + process_channels = [] + for channel in channels: + if channel not in layer_specs: + # If the channel is not in the layer_specs, we use the default fusion algorithm + matched_fusion = self.get_matching_fusion(channel, channel_fusions) + if matched_fusion is None: + if "default" in channel_fusions: + default_fusion = channel_fusions["default"] + print( + f"[WARNING] Layer {channel} not found in layer_specs. Using {default_fusion} algorithm as default." + ) + layer_specs[channel] = default_fusion + self.update_fusion_setting() + # If there's no default fusion algorithm, we skip this channel + else: + print( + f"[WARNING] Layer {channel} not found in layer_specs ({layer_specs}) and no default fusion is configured. Skipping." + ) + continue + else: + layer_specs[channel] = matched_fusion + self.update_fusion_setting() + x = layer_specs[channel] + fusion_list.append(x) + process_channels.append(channel) + return process_channels, fusion_list + + def get_matching_fusion(self, channel: str, fusion_algs: Dict[str, str]): + """ Use regular expression to check if the fusion algorithm matches the channel name.""" + for fusion_alg, alg_value in fusion_algs.items(): + if re.match(f"^{fusion_alg}$", channel): + return alg_value + return None + + def get_layer_indices(self, fusion_alg, layer_specs): + """Get the indices of the layers that are used for a specific fusion algorithm. + + Args: + fusion_alg(str): fusion algorithm name + + Returns: + cp.array: indices of the layers + """ + layer_indices = cp.array([], dtype=cp.int32) + for it, (key, val) in enumerate(layer_specs.items()): + if key in val == fusion_alg: + layer_indices = cp.append(layer_indices, it).astype(cp.int32) + return layer_indices + + def get_indices_fusion(self, pcl_channels: List[str], fusion_alg: str, layer_specs: Dict[str, str]): + """Computes the indices of the channels of the pointcloud and the layers of the semantic map of type fusion_alg. + + Args: + pcl_channels (List[str]): list of all channel names + fusion_alg (str): fusion algorithm type we want to use for channel selection + + Returns: + Union[Tuple[List[int], List[int]], Tuple[cupy._core.core.ndarray, cupy._core.core.ndarray]]: + + + """ + # this contains exactly the fusion alg type for each channel of the pcl + pcl_val_list = [layer_specs[x] for x in pcl_channels] + # this contains the indices of the point cloud where we have to perform a certain fusion + pcl_indices = cp.array([idp + 3 for idp, x in enumerate(pcl_val_list) if x == fusion_alg], dtype=cp.int32,) + # create a list of indices of the layers that will be updated by the point cloud with specific fusion alg + layer_indices = cp.array([], dtype=cp.int32) + for it, (key, val) in enumerate(layer_specs.items()): + if key in pcl_channels and val == fusion_alg: + layer_idx = self.layer_names.index(key) + layer_indices = cp.append(layer_indices, layer_idx).astype(cp.int32) + return pcl_indices, layer_indices + + def update_layers_pointcloud(self, points_all, channels, R, t, elevation_map): + """Update the semantic map with the pointcloud. + + Args: + points_all: semantic point cloud + channels: list of channel names + R: rotation matrix + t: translation vector + elevation_map: elevation map object + """ + process_channels, additional_fusion = self.get_fusion( + channels, self.param.pointcloud_channel_fusions, self.layer_specs_points + ) + # If channels has a new layer that is not in the semantic map, add it + for channel in process_channels: + if channel not in self.layer_names: + print(f"Layer {channel} not found, adding it to the semantic map") + self.add_layer(channel) + + # Resetting new_map for the layers that are to be deleted + self.new_map[self.delete_new_layers] = 0.0 + for fusion in list(set(additional_fusion)): + # which layers need to be updated with this fusion algorithm + pcl_ids, layer_ids = self.get_indices_fusion(process_channels, fusion, self.layer_specs_points) + # update the layers with the fusion algorithm + self.fusion_manager.execute_plugin( + fusion, + points_all, + R, + t, + pcl_ids, + layer_ids, + elevation_map, + self.semantic_map, + self.new_map, + self.elements_to_shift, + ) + + def update_layers_image( + self, + # sub_key: str, + image: cp._core.core.ndarray, + channels: List[str], + # fusion_methods: List[str], + uv_correspondence: cp._core.core.ndarray, + valid_correspondence: cp._core.core.ndarray, + image_height: cp._core.core.ndarray, + image_width: cp._core.core.ndarray, + ): + """Update the semantic map with the new image. + + Args: + sub_key: + image: + uv_correspondence: + valid_correspondence: + image_height: + image_width: + """ + + process_channels, fusion_methods = self.get_fusion( + channels, self.param.image_channel_fusions, self.layer_specs_image + ) + self.new_map[self.delete_new_layers] = 0.0 + for j, (fusion, channel) in enumerate(zip(fusion_methods, process_channels)): + if channel not in self.layer_names: + print(f"Layer {channel} not found, adding it to the semantic map") + self.add_layer(channel) + sem_map_idx = self.get_index(channel) + + if sem_map_idx == -1: + print(f"Layer {channel} not found!") + return + + # update the layers with the fusion algorithm + self.fusion_manager.execute_image_plugin( + fusion, + cp.uint64(sem_map_idx), + image, + j, + uv_correspondence, + valid_correspondence, + image_height, + image_width, + self.semantic_map, + self.new_map, + ) + + def decode_max(self, mer): + """Decode the float32 value into two 16 bit value containing the class probability and the class id. + + Args: + mer: + + Returns: + cp.array: probability + cp.array: class id + """ + mer = mer.astype(cp.float32) + mer = mer.view(dtype=cp.uint32) + ma = cp.bitwise_and(mer, 0xFFFF, dtype=np.uint16) + ma = ma.view(np.float16) + ma = ma.astype(np.float32) + ind = cp.right_shift(mer, 16) + return ma, ind + + def get_map_with_name(self, name): + """Return the map with the given name. + + Args: + name: layer name + + Returns: + cp.array: map + """ + # If the layer is a color layer, return the rgb map + if name in self.layer_specs_points and self.layer_specs_points[name] == "color": + m = self.get_rgb(name) + return m + elif name in self.layer_specs_image and self.layer_specs_image[name] == "color": + m = self.get_rgb(name) + return m + else: + m = self.get_semantic(name) + return m + + def get_rgb(self, name): + """Return the rgb map with the given name. + + Args: + name: + + Returns: + cp.array: rgb map + """ + idx = self.layer_names.index(name) + c = self.process_map_for_publish(self.semantic_map[idx]) + c = c.astype(np.float32) + return c + + def get_semantic(self, name): + """Return the semantic map layer with the given name. + + Args: + name(str): layer name + + Returns: + cp.array: semantic map layer + """ + idx = self.layer_names.index(name) + c = self.process_map_for_publish(self.semantic_map[idx]) + return c + + def process_map_for_publish(self, input_map): + """Remove padding. + + Args: + input_map(cp.array): map layer + + Returns: + cp.array: map layer without padding + """ + m = input_map.copy() + return m[1:-1, 1:-1] + + def get_index(self, name): + """Return the index of the layer with the given name. + + Args: + name(str): + + Returns: + int: index + """ + if name not in self.layer_names: + return -1 + else: + return self.layer_names.index(name) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/__init__.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_elevation_mapping.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_elevation_mapping.py new file mode 100644 index 00000000..7bdbb20b --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_elevation_mapping.py @@ -0,0 +1,118 @@ +import pytest +from elevation_mapping_cupy import parameter, elevation_mapping +import cupy as cp +import numpy as np + + +def encode_max(maxim, index): + maxim, index = cp.asarray(maxim, dtype=cp.float32), cp.asarray(index, dtype=cp.uint32) + # fuse them + maxim = maxim.astype(cp.float16) + maxim = maxim.view(cp.uint16) + maxim = maxim.astype(cp.uint32) + index = index.astype(cp.uint32) + mer = cp.array(cp.left_shift(index, 16) | maxim, dtype=cp.uint32) + mer = mer.view(cp.float32) + return mer + + +@pytest.fixture() +def elmap_ex(add_lay, fusion_alg): + additional_layer = add_lay + fusion_algorithms = fusion_alg + p = parameter.Parameter( + use_chainer=False, + weight_file="../../../config/weights.dat", + plugin_config_file="../../../config/plugin_config.yaml", + ) + p.subscriber_cfg["front_cam"]["channels"] = additional_layer + p.subscriber_cfg["front_cam"]["fusion"] = fusion_algorithms + p.update() + e = elevation_mapping.ElevationMap(p) + return e + + +@pytest.mark.parametrize( + "add_lay,fusion_alg", + [ + (["feat_0", "feat_1", "rgb"], ["average", "average", "color"]), + (["feat_0", "feat_1"], ["average", "average"]), + (["feat_0", "feat_1"], ["class_average", "class_average"]), + (["feat_0", "feat_1"], ["class_bayesian", "class_bayesian"]), + (["feat_0", "feat_1"], ["class_bayesian", "class_max"]), + (["feat_0", "feat_1"], ["bayesian_inference", "bayesian_inference"]), + ], +) +class TestElevationMap: + def test_init(self, elmap_ex): + assert len(elmap_ex.layer_names) == elmap_ex.elevation_map.shape[0] + # assert elmap_ex.color_map is None + + def test_input(self, elmap_ex): + channels = ["x", "y", "z"] + elmap_ex.param.additional_layers + if "class_max" in elmap_ex.param.fusion_algorithms: + val = cp.random.rand(100000, len(channels), dtype=cp.float32).astype(cp.float16) + ind = cp.random.randint(0, 2, (100000, len(channels)), dtype=cp.uint32).astype(cp.float32) + points = encode_max(val, ind) + else: + points = cp.random.rand(100000, len(channels), dtype=elmap_ex.param.data_type) + R = cp.random.rand(3, 3, dtype=elmap_ex.param.data_type) + t = cp.random.rand(3, dtype=elmap_ex.param.data_type) + elmap_ex.input_pointcloud(points, channels, R, t, 0, 0) + + def test_update_normal(self, elmap_ex): + elmap_ex.update_normal(elmap_ex.elevation_map[0]) + + def test_move_to(self, elmap_ex): + for i in range(20): + pos = np.array([i * 0.01, i * 0.02, i * 0.01]) + R = cp.random.rand(3, 3) + elmap_ex.move_to(pos, R) + + def test_get_map(self, elmap_ex): + layers = [ + "elevation", + "variance", + "traversability", + "min_filter", + "smooth", + "inpaint", + "rgb", + ] + data = np.zeros((elmap_ex.cell_n - 2, elmap_ex.cell_n - 2), dtype=cp.float32) + for layer in layers: + elmap_ex.get_map_with_name_ref(layer, data) + + def test_get_position(self, elmap_ex): + pos = np.random.rand(1, 3) + elmap_ex.get_position(pos) + + def test_clear(self, elmap_ex): + elmap_ex.clear() + + def test_move(self, elmap_ex): + delta_position = np.random.rand(3) + elmap_ex.move(delta_position) + + def test_exists_layer(self, elmap_ex, add_lay): + for layer in add_lay: + assert elmap_ex.exists_layer(layer) + + def test_polygon_traversability(self, elmap_ex): + polygon = cp.array([[0, 0], [2, 0], [0, 2]], dtype=np.float64) + result = np.array([0, 0, 0]) + number_polygons = elmap_ex.get_polygon_traversability(polygon, result) + untraversable_polygon = np.zeros((number_polygons, 2)) + elmap_ex.get_untraversable_polygon(untraversable_polygon) + + def test_initialize_map(self, elmap_ex): + methods = ["linear", "cubic", "nearest"] + for method in methods: + points = np.array([[-4.0, 0.0, 0.0], [-4.0, 8.0, 1.0], [4.0, 8.0, 0.0], [4.0, 0.0, 0.0]]) + elmap_ex.initialize_map(points, method) + + def test_plugins(self, elmap_ex): + layers = elmap_ex.plugin_manager.layer_names + data = np.zeros((200, 200), dtype=np.float32) + for layer in layers: + elmap_ex.get_map_with_name_ref(layer, data) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_parameter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_parameter.py new file mode 100644 index 00000000..4c497349 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_parameter.py @@ -0,0 +1,17 @@ +import pytest +from elevation_mapping_cupy.parameter import Parameter + + +def test_parameter(): + param = Parameter( + use_chainer=False, + weight_file="../../../config/weights.dat", + plugin_config_file="../../../config/plugin_config.yaml", + ) + res = param.resolution + param.set_value("resolution", 0.1) + param.get_types() + param.get_names() + param.update() + assert param.resolution == param.get_value("resolution") + param.load_weights(param.weight_file) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_plugins.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_plugins.py new file mode 100644 index 00000000..f3e8cc1c --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_plugins.py @@ -0,0 +1,69 @@ +import pytest +from elevation_mapping_cupy import semantic_map, parameter +import cupy as cp +import numpy as np +from elevation_mapping_cupy.plugins.plugin_manager import PluginManager, PluginParams + +plugin_path = "plugin_config.yaml" + + +@pytest.fixture() +def semmap_ex(add_lay, fusion_alg): + p = parameter.Parameter( + use_chainer=False, weight_file="../../../config/weights.dat", plugin_config_file=plugin_path, + ) + p.subscriber_cfg["front_cam"]["channels"] = add_lay + p.subscriber_cfg["front_cam"]["fusion"] = fusion_alg + p.update() + e = semantic_map.SemanticMap(p) + e.initialize_fusion() + return e + + +@pytest.mark.parametrize( + "add_lay, fusion_alg,channels", + [ + ( + ["grass", "tree", "fence", "person"], + ["class_average", "class_average", "class_average", "class_average"], + ["grass"], + ), + (["grass", "tree"], ["class_average", "class_average"], ["grass"]), + (["grass", "tree"], ["class_average", "class_max"], ["tree"]), + (["max1", "max2"], ["class_max", "class_max"], ["max1", "max2"]), + ], +) +def test_plugin_manager(semmap_ex, channels): + manager = PluginManager(202) + manager.load_plugin_settings(plugin_path) + elevation_map = cp.zeros((7, 202, 202)).astype(cp.float32) + rotation = cp.eye(3, dtype=cp.float32) + layer_names = [ + "elevation", + "variance", + "is_valid", + "traversability", + "time", + "upper_bound", + "is_upper_bound", + ] + elevation_map[0] = cp.random.randn(202, 202) + elevation_map[2] = cp.abs(cp.random.randn(202, 202)) + elevation_map[0] + manager.layers[0] + manager.update_with_name("min_filter", elevation_map, layer_names) + manager.update_with_name("smooth_filter", elevation_map, layer_names) + manager.update_with_name("semantic_filter", elevation_map, layer_names, semmap_ex, rotation) + manager.update_with_name("semantic_traversability", elevation_map, layer_names, semmap_ex) + manager.get_map_with_name("smooth") + for lay in manager.get_layer_names(): + manager.update_with_name( + lay, + elevation_map, + layer_names, + semmap_ex.semantic_map, + semmap_ex.param, + rotation, + semmap_ex.elements_to_shift, + ) + manager.get_map_with_name(lay) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_kernels.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_kernels.py new file mode 100644 index 00000000..590946eb --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_kernels.py @@ -0,0 +1,307 @@ +import pytest +from elevation_mapping_cupy import parameter, elevation_mapping +import cupy as cp + +from elevation_mapping_cupy.parameter import Parameter + +from elevation_mapping_cupy.kernels import add_points_kernel +from elevation_mapping_cupy.kernels import ( + average_kernel, + class_average_kernel, + alpha_kernel, + bayesian_inference_kernel, + add_color_kernel, + color_average_kernel, + sum_compact_kernel, + sum_max_kernel, + sum_kernel, +) + + +# to check output run: pytest -rP test_semantic_kernels.py + + +# only kernel where we test only one layer +def test_color_kernel(): + # params + cell_n = 4 + pcl_ids, layer_ids = cp.array([3], dtype=cp.int32), cp.array([0], dtype=cp.int32) + R = cp.eye(3, dtype=cp.float32) + t = cp.array([0, 0, 0], dtype=cp.float32) + points_all = cp.array([[0.1, 0.1, 0.1, 0.3], [0.1, 0.1, 0.1, 0.1], [0.1, 0.1, 0.1, 0.1]], dtype=cp.float32) + semantic_map = cp.zeros((1, 4, 4), dtype=cp.float32) + + # compile kernel + add_color_kernel_ = add_color_kernel(cell_n, cell_n,) + color_average_kernel_ = color_average_kernel(cell_n, cell_n) + + # updatelayer + color_map = cp.zeros((1 + 3 * layer_ids.shape[0], cell_n, cell_n), dtype=cp.uint32,) + + points_all = points_all.astype(cp.float32) + add_color_kernel_( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + color_map, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + color_average_kernel_( + color_map, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + semantic_map, + size=(cell_n * cell_n * pcl_ids.shape[0]), + ) + print(color_map) + + +@pytest.mark.parametrize( + "map_shape, points_all,pcl_ids, layer_ids", + [ + ( + (4, 4, 4), + cp.array( + [[0.1, 0.1, 0.1, 0.3, 0.3], [0.1, 0.1, 0.1, 0.1, 0.2], [0.1, 0.1, 0.1, 0.1, 0.2]], dtype=cp.float32 + ), + cp.array([3, 4], dtype=cp.int32), + cp.array([1, 2], dtype=cp.int32), + ), + ], +) +def test_sum_kernel(map_shape, points_all, pcl_ids, layer_ids): + # create points + resolution = 0.9 + points = points_all[:, :3] + # arguments for kernel + semantic_map = cp.zeros(map_shape, dtype=cp.float32) + new_map = cp.zeros(map_shape, dtype=cp.float32) + R = cp.eye(3, dtype=cp.float32) + t = cp.array([0, 0, 0], dtype=cp.float32) + # compile kernel + sum_kernel_ = sum_kernel(0.9, 4, 4,) + # simulating adding the points + print("idx, valid, inside, values") + points_all[:, 0] = 1 + points_all[:, 1:3] = 1.0 + print("points all after ", points_all) + # run the kernel + sum_kernel_( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + semantic_map, + new_map, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + print("semantic_map", semantic_map) + print("new_map", new_map) + + +@pytest.mark.parametrize( + "map_shape, points_all,pcl_ids, layer_ids", + [ + ( + (4, 4, 4), + cp.array( + [[0.1, 0.1, 0.1, 0.3, 0.3], [0.1, 0.1, 0.1, 0.1, 0.2], [0.1, 0.1, 0.1, 0.1, 0.2]], dtype=cp.float32 + ), + cp.array([3, 4], dtype=cp.int32), + cp.array([1, 2], dtype=cp.int32), + ), + ], +) +def test_average_kernel(map_shape, points_all, pcl_ids, layer_ids): + elevation_map = cp.zeros((3, 4, 4), dtype=cp.float32) + elevation_map[2] = points_all.shape[0] + semantic_map = cp.zeros(map_shape, dtype=cp.float32) + new_map = cp.ones(map_shape, dtype=cp.float32) + R = cp.eye(3, dtype=cp.float32) + t = cp.array([0, 0, 0], dtype=cp.float32) + # compile kernel + average_kernel_ = average_kernel(4, 4,) + cell_n = 4 + print("new_map", new_map) + print("semantic_map", semantic_map) + print("elevation_map", elevation_map) + + average_kernel_( + new_map, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + elevation_map, + semantic_map, + size=(cell_n * cell_n * pcl_ids.shape[0]), + ) + print("semantic_map", semantic_map) + + +@pytest.mark.parametrize( + "map_shape, points_all,pcl_ids, layer_ids", + [ + ( + (3, 4, 4), + cp.array( + [[0.1, 0.1, 0.1, 0.3, 0.3], [0.1, 0.1, 0.1, 0.1, 0.2], [0.1, 0.1, 0.1, 0.1, 0.2]], dtype=cp.float32 + ), + cp.array([3], dtype=cp.int32), + cp.array([0], dtype=cp.int32), + ), + ], +) +def test_bayesian_inference_kernel(map_shape, points_all, pcl_ids, layer_ids): + # params + cell_n = 4 + resolution = 0.9 + elevation_map = cp.zeros((3, 4, 4), dtype=cp.float32) + elevation_map[2] = points_all.shape[0] + semantic_map = cp.zeros(map_shape, dtype=cp.float32) + new_map = cp.ones(map_shape, dtype=cp.float32) + R = cp.eye(3, dtype=cp.float32) + t = cp.array([0, 0, 0], dtype=cp.float32) + + # compile kernel + sum_mean = cp.ones((pcl_ids.shape[0], cell_n, cell_n,), cp.float32,) + sum_compact_kernel_ = sum_compact_kernel(resolution, cell_n, cell_n,) + bayesian_inference_kernel_ = bayesian_inference_kernel(cell_n, cell_n,) + # updatelayer + sum_mean *= 0 + sum_compact_kernel_( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + sum_mean, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + bayesian_inference_kernel_( + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + elevation_map, + new_map, + sum_mean, + semantic_map, + size=(cell_n * cell_n * pcl_ids.shape[0]), + ) + print("semantic_map", semantic_map) + + +@pytest.mark.parametrize( + "map_shape, points_all,pcl_ids, layer_ids", + [ + ( + (4, 4, 4), + cp.array( + [[0.1, 0.1, 0.1, 0.3, 0.3], [0.1, 0.1, 0.1, 0.1, 0.2], [0.1, 0.1, 0.1, 0.1, 0.2]], dtype=cp.float32 + ), + cp.array([3, 4], dtype=cp.int32), + cp.array([1, 2], dtype=cp.int32), + ), + ], +) +def test_class_average_kernel(map_shape, points_all, pcl_ids, layer_ids): + # params + cell_n = 4 + resolution = 0.9 + average_weight = 0.5 + elevation_map = cp.zeros((3, 4, 4), dtype=cp.float32) + elevation_map[2] = 3 + semantic_map = cp.zeros(map_shape, dtype=cp.float32) + new_map = cp.zeros(map_shape, dtype=cp.float32) + R = cp.eye(3, dtype=cp.float32) + t = cp.array([0, 0, 0], dtype=cp.float32) + # compile kernel + sum_kernel_ = sum_kernel(0.9, 4, 4,) + class_average_kernel_ = class_average_kernel(cell_n, cell_n, average_weight,) + print("x,y,z,class") + print("points all after ", points_all) + + # simulating adding the points + print("idx, valid, inside, values") + points_all[:, 0] = 1 + points_all[:, 1:3] = 1.0 + print("points all after ", points_all) + # run the kernel + sum_kernel_( + points_all, + R, + t, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + semantic_map, + new_map, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + print("new_map bef", new_map) + print("pcl_ids.shape[0]", pcl_ids.shape[0]) + class_average_kernel_( + new_map, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + elevation_map, + semantic_map, + size=(cell_n * cell_n * pcl_ids.shape[0]), + ) + print("semantic_map", semantic_map) + print("new_map", new_map) + + +@pytest.mark.parametrize( + "map_shape, points_all,pcl_ids, layer_ids", + [ + ( + (4, 4, 4), + cp.array( + [[0.1, 0.1, 0.1, 0.3, 0.3], [0.1, 0.1, 0.1, 0.1, 0.2], [0.1, 0.1, 0.1, 0.1, 0.2]], dtype=cp.float32 + ), + cp.array([3, 4], dtype=cp.int32), + cp.array([1, 2], dtype=cp.int32), + ), + ], +) +def test_class_bayesian_inference_fct(map_shape, points_all, pcl_ids, layer_ids): + # params + cell_n = 4 + resolution = 0.9 + elevation_map = cp.zeros((3, 4, 4), dtype=cp.float32) + elevation_map[2] = points_all.shape[0] + semantic_map = cp.zeros(map_shape, dtype=cp.float32) + new_map = cp.zeros(map_shape, dtype=cp.float32) + R = cp.eye(3, dtype=cp.float32) + t = cp.array([0, 0, 0], dtype=cp.float32) + # compile kernel + alpha_kernel_ = alpha_kernel(resolution, cell_n, cell_n,) + # simulating adding the points + print("idx, valid, inside, values") + points_all[:, 0] = 1 + points_all[:, 1:3] = 1.0 + print("points all after ", points_all) + # run the kernel + alpha_kernel_( + points_all, + pcl_ids, + layer_ids, + cp.array([points_all.shape[1], pcl_ids.shape[0]], dtype=cp.int32), + new_map, + size=(points_all.shape[0] * pcl_ids.shape[0]), + ) + # calculate new thetas + sum_alpha = cp.sum(new_map[layer_ids], axis=0) + # do not divide by zero + sum_alpha[sum_alpha == 0] = 1 + semantic_map[layer_ids] = new_map[layer_ids] / cp.expand_dims(sum_alpha, axis=0) + print("semantic_map", semantic_map) + print("new_map", new_map) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_map.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_map.py new file mode 100644 index 00000000..95966bc4 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/tests/test_semantic_map.py @@ -0,0 +1,47 @@ +import pytest +from elevation_mapping_cupy import semantic_map, parameter +import cupy as cp +import numpy as np + + +@pytest.fixture() +def semmap_ex(sem_lay, fusion_alg): + p = parameter.Parameter( + use_chainer=False, + weight_file="../../../config/weights.dat", + plugin_config_file="../../../config/plugin_config.yaml", + ) + for subs, value in p.subscriber_cfg.items(): + value["channels"] = sem_lay + value["fusion"] = fusion_alg + p.update() + e = semantic_map.SemanticMap(p) + return e + + +@pytest.mark.parametrize( + "sem_lay, fusion_alg,channels", + [ + (["feat_0", "feat_1"], ["average", "average"], ["feat_0"]), + (["feat_0", "feat_1"], ["average", "average"], []), + (["feat_0", "feat_1", "rgb"], ["average", "average", "color"], ["rgb", "feat_0"],), + (["feat_0", "feat_1", "rgb"], ["class_average", "average", "color"], ["rgb", "feat_0"],), + (["feat_0", "feat_1", "rgb"], ["class_bayesian", "average", "color"], ["rgb", "feat_0"],), + (["feat_0", "feat_1", "rgb"], ["class_bayesian", "average", "color"], ["rgb", "feat_0", "feat_1"],), + (["feat_0", "feat_1", "rgb"], ["class_bayesian", "class_max", "color"], ["rgb", "feat_0", "feat_1"],), + (["max1", "max2", "rgb"], ["class_max", "class_max", "color"], ["rgb", "max1", "max2"],), + ], +) +def test_fusion_of_pcl(semmap_ex, channels): + fusion = semmap_ex.get_fusion_of_pcl(channels=channels) + assert len(fusion) <= len(channels) + assert len(fusion) > 0 or len(channels) == 0 + assert all(isinstance(item, str) for item in fusion) + + +@pytest.mark.parametrize( + "sem_lay, fusion_alg", [(["feat_0", "feat_1", "rgb"], ["average", "average", "color"]),], +) +@pytest.mark.parametrize("channels", [["rgb"], ["rgb", "feat_0"], []]) +def test_indices_fusion(semmap_ex, channels, fusion_alg): + pcl_indices, layer_indices = semmap_ex.get_indices_fusion(pcl_channels=channels, fusion_alg=fusion_alg[0]) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_filter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_filter.py new file mode 100644 index 00000000..7dfb47c2 --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_filter.py @@ -0,0 +1,100 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp + + +def get_filter_torch(*args, **kwargs): + import torch + import torch.nn as nn + + class TraversabilityFilter(nn.Module): + def __init__(self, w1, w2, w3, w_out, device="cuda", use_bias=False): + super(TraversabilityFilter, self).__init__() + self.conv1 = nn.Conv2d(1, 4, 3, dilation=1, padding=0, bias=use_bias) + self.conv2 = nn.Conv2d(1, 4, 3, dilation=2, padding=0, bias=use_bias) + self.conv3 = nn.Conv2d(1, 4, 3, dilation=3, padding=0, bias=use_bias) + self.conv_out = nn.Conv2d(12, 1, 1, bias=use_bias) + + # Set weights. + self.conv1.weight = nn.Parameter(torch.from_numpy(w1).float()) + self.conv2.weight = nn.Parameter(torch.from_numpy(w2).float()) + self.conv3.weight = nn.Parameter(torch.from_numpy(w3).float()) + self.conv_out.weight = nn.Parameter(torch.from_numpy(w_out).float()) + + def __call__(self, elevation_cupy): + # Convert cupy tensor to pytorch. + elevation_cupy = elevation_cupy.astype(cp.float32) + elevation = torch.as_tensor(elevation_cupy, device=self.conv1.weight.device) + + with torch.no_grad(): + out1 = self.conv1(elevation.view(-1, 1, elevation.shape[0], elevation.shape[1])) + out2 = self.conv2(elevation.view(-1, 1, elevation.shape[0], elevation.shape[1])) + out3 = self.conv3(elevation.view(-1, 1, elevation.shape[0], elevation.shape[1])) + + out1 = out1[:, :, 2:-2, 2:-2] + out2 = out2[:, :, 1:-1, 1:-1] + out = torch.cat((out1, out2, out3), dim=1) + # out = F.concat((out1, out2, out3), axis=1) + out = self.conv_out(out.abs()) + out = torch.exp(-out) + out_cupy = cp.asarray(out) + + return out_cupy + + traversability_filter = TraversabilityFilter(*args, **kwargs).cuda().eval() + return traversability_filter + + +def get_filter_chainer(*args, **kwargs): + import os + + os.environ["CHAINER_WARN_VERSION_MISMATCH"] = "0" + import chainer + import chainer.links as L + import chainer.functions as F + + class TraversabilityFilter(chainer.Chain): + def __init__(self, w1, w2, w3, w_out, use_cupy=True): + super(TraversabilityFilter, self).__init__() + self.conv1 = L.Convolution2D(1, 4, ksize=3, pad=0, dilate=1, nobias=True, initialW=w1) + self.conv2 = L.Convolution2D(1, 4, ksize=3, pad=0, dilate=2, nobias=True, initialW=w2) + self.conv3 = L.Convolution2D(1, 4, ksize=3, pad=0, dilate=3, nobias=True, initialW=w3) + self.conv_out = L.Convolution2D(12, 1, ksize=1, nobias=True, initialW=w_out) + + if use_cupy: + self.conv1.to_gpu() + self.conv2.to_gpu() + self.conv3.to_gpu() + self.conv_out.to_gpu() + chainer.config.train = False + chainer.config.enable_backprop = False + + def __call__(self, elevation): + out1 = self.conv1(elevation.reshape(-1, 1, elevation.shape[0], elevation.shape[1])) + out2 = self.conv2(elevation.reshape(-1, 1, elevation.shape[0], elevation.shape[1])) + out3 = self.conv3(elevation.reshape(-1, 1, elevation.shape[0], elevation.shape[1])) + + out1 = out1[:, :, 2:-2, 2:-2] + out2 = out2[:, :, 1:-1, 1:-1] + out = F.concat((out1, out2, out3), axis=1) + out = self.conv_out(F.absolute(out)) + return F.exp(-out).array + + traversability_filter = TraversabilityFilter(*args, **kwargs) + return traversability_filter + + +if __name__ == "__main__": + import cupy as cp + from parameter import Parameter + + elevation = cp.random.randn(202, 202, dtype=cp.float32) + print("elevation ", elevation.shape) + param = Parameter() + fc = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) + print("chainer ", fc(elevation)) + + ft = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) + print("torch ", ft(elevation)) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_polygon.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_polygon.py new file mode 100644 index 00000000..be874f1b --- /dev/null +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/traversability_polygon.py @@ -0,0 +1,77 @@ +# +# Copyright (c) 2022, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import numpy as np +import cupy as cp +from shapely.geometry import Polygon, MultiPoint + + +def get_masked_traversability(map_array, mask, traversability): + traversability = traversability[1:-1, 1:-1] + is_valid = map_array[2][1:-1, 1:-1] + mask = mask[1:-1, 1:-1] + + # invalid place is 0 traversability value + untraversability = cp.where(is_valid > 0.5, 1 - traversability, 0) + masked = untraversability * mask + masked_isvalid = is_valid * mask + return masked, masked_isvalid + + +def is_traversable(masked_untraversability, thresh, min_thresh, max_over_n): + untraversable_thresh = 1 - thresh + max_thresh = 1 - min_thresh + over_thresh = cp.where(masked_untraversability > untraversable_thresh, 1, 0) + polygon = calculate_untraversable_polygon(over_thresh) + max_untraversability = masked_untraversability.max() + if over_thresh.sum() > max_over_n: + is_safe = False + elif max_untraversability > max_thresh: + is_safe = False + else: + is_safe = True + return is_safe, polygon + + +def calculate_area(polygon): + area = 0 + for i in range(len(polygon)): + p1 = polygon[i - 1] + p2 = polygon[i] + area += (p1[0] * p2[1] - p1[1] * p2[0]) / 2.0 + return abs(area) + + +def calculate_untraversable_polygon(over_thresh): + x, y = cp.where(over_thresh > 0.5) + points = cp.stack([x, y]).T + convex_hull = MultiPoint(points.get()).convex_hull + if convex_hull.is_empty or convex_hull.geom_type == "Point" or convex_hull.geom_type == "LineString": + return None + else: + return cp.array(convex_hull.exterior.coords) + + +def transform_to_map_position(polygon, center, cell_n, resolution): + polygon = center.reshape(1, 2) + (polygon - cell_n / 2.0) * resolution + return polygon + + +def transform_to_map_index(points, center, cell_n, resolution): + indices = ((points - center.reshape(1, 2)) / resolution + cell_n / 2).astype(cp.int32) + return indices + + +if __name__ == "__main__": + polygon = [[0, 0], [2, 0], [0, 2]] + print(calculate_area(polygon)) + + under_thresh = cp.zeros((20, 20)) + # under_thresh[10:12, 8:10] = 1.0 + under_thresh[14:18, 8:10] = 1.0 + under_thresh[1:8, 2:9] = 1.0 + print(under_thresh) + polygon = calculate_untraversable_polygon(under_thresh) + print(polygon) + transform_to_map_position(polygon, cp.array([0.5, 1.0]), 6.0, 0.05) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 17baea09..c9228427 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,72 +1,176 @@ -#### Basic parameters ######## -resolution: 0.04 # resolution in m. -map_length: 8.0 # map's size in m. -sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). -mahalanobis_thresh: 2.0 # points outside this distance is outlier. -outlier_variance: 0.01 # if point is outlier, add this value to the cell. -drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation. -max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety) -drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation -time_variance: 0.0001 # add this value when update_variance is called. -max_variance: 100.0 # maximum variance for each cell. -initial_variance: 1000.0 # initial variance for each cell. -traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation. -dilation_size: 3 # dilation filter size before traversability filter. -wall_num_thresh: 20 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. -min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number. -position_noise_thresh: 0.01 # if the position change is bigger than this value, the drift compensation happens. -orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens. -position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. -orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. -min_valid_distance: 0.5 # points with shorter distance will be filtered out. -max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling. -ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. -ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. -ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. -update_variance_fps: 5.0 # fps for updating variance. -update_pose_fps: 10.0 # fps for updating pose and shift the center of map. -time_interval: 0.1 # Time layer is updated with this interval. -map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps. -publish_statistics_fps: 1.0 # Publish statistics topic in this fps. - -max_ray_length: 10.0 # maximum length for ray tracing. -cleanup_step: 0.1 # subtitute this value from validity layer at visibiltiy cleanup. -cleanup_cos_thresh: 0.1 # subtitute this value from validity layer at visibiltiy cleanup. - -safe_thresh: 0.7 # if traversability is smaller, it is counted as unsafe cell. -safe_min_thresh: 0.4 # polygon is unsafe if there exists lower traversability than this. -max_unsafe_n: 10 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe. - -overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) -overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) - -map_frame: 'odom' # The map frame where the odometry source uses. -base_frame: 'base_footprint' # The robot's base frame. This frame will be a center of the map. -corrected_map_frame: 'odom' - -#### Feature toggles ######## -enable_edge_sharpen: true -enable_visibility_cleanup: true -enable_drift_compensation: true -enable_overlap_clearance: true -enable_pointcloud_publishing: false -enable_drift_corrected_TF_publishing: false -enable_normal_color: false # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. - -#### Traversability filter ######## -use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. -weight_file: '$(rospack find elevation_mapping_cupy)/config/core/weights.dat' # Weight file for traversability filter - -#### Upper bound ######## -use_only_above_for_upper_bound: false - -#### Initializer ######## -initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic' -initialize_frame_id: ['base_footprint'] # One tf (like ['footprint'] ) initializes a square around it. -initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id. -dilation_size_initialize: 2 # dilation size after the init. -initialize_tf_grid_size: 0.5 # This is not used if number of tf is more than 3. -use_initializer_at_start: true # Use initializer when the node starts. - -#### Default Plugins ######## -plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/core/plugin_config.yaml' \ No newline at end of file +elevation_mapping: + ros__parameters: + #### Basic parameters ######## + resolution: 0.04 # resolution in m. + map_length: 8.0 # map's size in m. + sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). + mahalanobis_thresh: 2.0 # points outside this distance is outlier. + outlier_variance: 0.01 # if point is outlier, add this value to the cell. + drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation. + max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety) + drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation + time_variance: 0.0001 # add this value when update_variance is called. + max_variance: 100.0 # maximum variance for each cell. + initial_variance: 1000.0 # initial variance for each cell. + traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation. + dilation_size: 3 # dilation filter size before traversability filter. + wall_num_thresh: 20 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. + min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number. + position_noise_thresh: 0.01 # if the position change is bigger than this value, the drift compensation happens. + orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens. + position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. + orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. + min_valid_distance: 0.5 # points with shorter distance will be filtered out. + max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling. + ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + update_variance_fps: 5.0 # fps for updating variance. + update_pose_fps: 10.0 # fps for updating pose and shift the center of map. + time_interval: 0.1 # Time layer is updated with this interval. + map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps. + publish_statistics_fps: 1.0 # Publish statistics topic in this fps. + + max_ray_length: 10.0 # maximum length for ray tracing. + cleanup_step: 0.1 # subtitute this value from validity layer at visibiltiy cleanup. + cleanup_cos_thresh: 0.1 # subtitute this value from validity layer at visibiltiy cleanup. + + safe_thresh: 0.7 # if traversability is smaller, it is counted as unsafe cell. + safe_min_thresh: 0.4 # polygon is unsafe if there exists lower traversability than this. + max_unsafe_n: 10 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe. + + overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) + overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) + pose_topic: '/odom' + map_frame: 'base_footprint' # The map frame where the odometry source uses. + base_frame: 'base_link' # The robot's base frame. This frame will be a center of the map. + corrected_map_frame: 'odom' + + + + #### Feature toggles ######## + enable_edge_sharpen: true + enable_visibility_cleanup: true + enable_drift_compensation: true + enable_overlap_clearance: true + enable_pointcloud_publishing: false + enable_drift_corrected_TF_publishing: false + enable_normal_color: false # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. + + #### Traversability filter ######## + use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. + weight_file: '/config/core/weights.dat' # Weight file for traversability filter + + #### Upper bound ######## + use_only_above_for_upper_bound: false + + #### Initializer ######## + initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic' + initialize_frame_id: ['base_footprint'] # One tf (like ['footprint'] ) initializes a square around it. + initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id. + dilation_size_initialize: 2 # dilation size after the init. + initialize_tf_grid_size: 0.5 # This is not used if number of tf is more than 3. + use_initializer_at_start: true # Use initializer when the node starts. + + #### Default Plugins ######## + + pointcloud_channel_fusions: + rgb: 'color' # 'color' fusion is used for the 'rgb' channel + default: 'average' # 'average' fusion is used for channels not listed here + + image_channel_fusions: + rgb: 'color' # 'color' fusion is used for the 'rgb' channel + default: 'exponential' # 'exponential' fusion is used for channels not listed here + feat_.*: 'exponential' # 'exponential' fusion is also used for any channel starting with 'feat_' Regular expressions can be used for channel names + + #### Subscribers ######## + # pointcloud_sensor_name: + # topic_name: '/sensor/pointcloud_semantic' + # data_type: pointcloud # pointcloud or image + # + # image_sensor_name: + # topic_name: '/camera/image_semantic' + # data_type: image # pointcloud or image + # camera_info_topic_name: '/camera/depth/camera_info' + # channel_info_topic_name: '/camera/channel_info' + + subscribers: + pointcloud_topic1: '/a200/sensors/camera_1/points' + + image_topic1: '/a200/sensors/camera_0/color/image' + image_info1: '/a200/sensors/camera_0/color/camera_info' + # image_channel_info1: '/front_cam/channel_info' + + # image_topic2: '/camera/rgb/image_raw2' + # image_info2: '/camera/depth/camera_info2' + # image_channel_info1: '/front_cam/channel_info' + + + + # image: # for semantic images + # topic_name: '/front_cam/semantic_image' + # camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized' + # channel_info_topic_name: '/front_cam/channel_info' + # data_type: image + + #### Publishers ######## + # topic_name: + # layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names + # basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`. + # fps: # Publish rate. Use smaller value than `map_acquire_fps`. + + publishers: + elevation_map_raw: + layers: ['elevation', 'traversability', 'variance','rgb'] + basic_layers: ['elevation'] + fps: 5.0 + elevation_map_recordable: + layers: ['elevation', 'traversability'] + basic_layers: ['elevation', 'traversability'] + fps: 2.0 + elevation_map_filter: + layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] + basic_layers: ['min_filter'] + fps: 3.0 + + + # plugun info + + min_filter: + enable: True # whether to load this plugin + fill_nan: False # Fill nans to invalid cells of elevation layer. + is_height_layer: True # If this is a height layer (such as elevation) or not (such as traversability) + layer_name: "min_filter" # The layer name. + extra_params: # These params are passed to the plugin class on initialization. + dilation_size: 1 # The patch size to apply + iteration_n: 30 # The number of iterations + + # Apply smoothing. + smooth_filter: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "smooth" + extra_params: + input_layer_name: "min_filter" + + # Apply inpainting using opencv + inpainting: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "inpaint" + extra_params: + method: "telea" # telea or ns + + # Apply smoothing for inpainted layer + erosion: + enable: True + fill_nan: False + is_height_layer: False + layer_name: "erosion" + extra_params: + input_layer_name: "traversability" + dilation_size: 3 + iteration_n: 20 + reverse: True \ No newline at end of file diff --git a/elevation_mapping_cupy/config/core/example_setup.yaml b/elevation_mapping_cupy/config/core/example_setup.yaml index a3f7a2f2..2fc0ee84 100644 --- a/elevation_mapping_cupy/config/core/example_setup.yaml +++ b/elevation_mapping_cupy/config/core/example_setup.yaml @@ -1,58 +1,101 @@ -#### Plugins ######## -plugin_config_file: '$(rospack find elevation_mapping_cupy)/config/core/plugin_config.yaml' - -#### Channel Fusions ######## -pointcloud_channel_fusions: - rgb: 'color' # 'color' fusion is used for the 'rgb' channel - default: 'average' # 'average' fusion is used for channels not listed here - -image_channel_fusions: - rgb: 'color' # 'color' fusion is used for the 'rgb' channel - default: 'exponential' # 'exponential' fusion is used for channels not listed here - feat_.*: 'exponential' # 'exponential' fusion is also used for any channel starting with 'feat_' Regular expressions can be used for channel names - -#### Subscribers ######## -# pointcloud_sensor_name: -# topic_name: '/sensor/pointcloud_semantic' -# data_type: pointcloud # pointcloud or image -# -# image_sensor_name: -# topic_name: '/camera/image_semantic' -# data_type: image # pointcloud or image -# camera_info_topic_name: '/camera/depth/camera_info' -# channel_info_topic_name: '/camera/channel_info' - - -subscribers: - front_cam: - topic_name: '/camera/depth/points' - data_type: pointcloud - color_cam: # for color camera - topic_name: '/camera/rgb/image_raw' - camera_info_topic_name: '/camera/depth/camera_info' - data_type: image - semantic_cam: # for semantic images - topic_name: '/front_cam/semantic_image' - camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized' - channel_info_topic_name: '/front_cam/channel_info' - data_type: image - -#### Publishers ######## -# topic_name: -# layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names -# basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`. -# fps: # Publish rate. Use smaller value than `map_acquire_fps`. - -publishers: - elevation_map_raw: - layers: ['elevation', 'traversability', 'variance','rgb'] - basic_layers: ['elevation'] - fps: 5.0 - elevation_map_recordable: - layers: ['elevation', 'traversability'] - basic_layers: ['elevation', 'traversability'] - fps: 2.0 - elevation_map_filter: - layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] - basic_layers: ['min_filter'] - fps: 3.0 \ No newline at end of file +elevation_mapping: + ros__parameters: + #### Plugins ######## + plugin_config_file: '$(find_package_share elevation_mapping_cupy)/config/core/plugin_config.yaml' + + #### Channel Fusions ######## + pointcloud_channel_fusions: + rgb: 'color' # 'color' fusion is used for the 'rgb' channel + default: 'average' # 'average' fusion is used for channels not listed here + + image_channel_fusions: + rgb: 'color' # 'color' fusion is used for the 'rgb' channel + default: 'exponential' # 'exponential' fusion is used for channels not listed here + feat_.*: 'exponential' # 'exponential' fusion is also used for any channel starting with 'feat_' Regular expressions can be used for channel names + + #### Subscribers ######## + # pointcloud_sensor_name: + # topic_name: '/sensor/pointcloud_semantic' + # data_type: pointcloud # pointcloud or image + # + # image_sensor_name: + # topic_name: '/camera/image_semantic' + # data_type: image # pointcloud or image + # camera_info_topic_name: '/camera/depth/camera_info' + # channel_info_topic_name: '/camera/channel_info' + + subscribers: + front_cam: + topic_name: '/camera/depth/points' + data_type: pointcloud + color_cam: # for color camera + topic_name: '/camera/rgb/image_raw' + camera_info_topic_name: '/camera/depth/camera_info' + data_type: image + semantic_cam: # for semantic images + topic_name: '/front_cam/semantic_image' + camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized' + channel_info_topic_name: '/front_cam/channel_info' + data_type: image + + #### Publishers ######## + # topic_name: + # layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names + # basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`. + # fps: # Publish rate. Use smaller value than `map_acquire_fps`. + + publishers: + elevation_map_raw: + layers: ['elevation', 'traversability', 'variance','rgb'] + basic_layers: ['elevation'] + fps: 5.0 + elevation_map_recordable: + layers: ['elevation', 'traversability'] + basic_layers: ['elevation', 'traversability'] + fps: 2.0 + elevation_map_filter: + layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] + basic_layers: ['min_filter'] + fps: 3.0 + + +# plugun info + + min_filter: + enable: True # whether to load this plugin + fill_nan: False # Fill nans to invalid cells of elevation layer. + is_height_layer: True # If this is a height layer (such as elevation) or not (such as traversability) + layer_name: "min_filter" # The layer name. + extra_params: # These params are passed to the plugin class on initialization. + dilation_size: 1 # The patch size to apply + iteration_n: 30 # The number of iterations + + # Apply smoothing. + smooth_filter: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "smooth" + extra_params: + input_layer_name: "min_filter" + + # Apply inpainting using opencv + inpainting: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "inpaint" + extra_params: + method: "telea" # telea or ns + + # Apply smoothing for inpainted layer + erosion: + enable: True + fill_nan: False + is_height_layer: False + layer_name: "erosion" + extra_params: + input_layer_name: "traversability" + dilation_size: 3 + iteration_n: 20 + reverse: True \ No newline at end of file diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index f631fbba..18fd4eaf 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -26,8 +26,11 @@ #include #include #include -#include "std_srvs/srv/empty.h" +#include #include +#include +#include +#include #include #include @@ -58,46 +61,50 @@ namespace py = pybind11; -// namespace elevation_mapping_cupy { - -// class ElevationMappingNode { -// public: -// ElevationMappingNode(ros::NodeHandle& nh); -// using RowMatrixXd = Eigen::Matrix; -// using ColMatrixXf = Eigen::Matrix; - -// using ImageSubscriber = image_transport::SubscriberFilter; -// using ImageSubscriberPtr = std::shared_ptr; - -// // Subscriber and Synchronizer for CameraInfo messages -// using CameraInfoSubscriber = message_filters::Subscriber; -// using CameraInfoSubscriberPtr = std::shared_ptr; -// using CameraPolicy = message_filters::sync_policies::ApproximateTime; -// using CameraSync = message_filters::Synchronizer; -// using CameraSyncPtr = std::shared_ptr; - -// // Subscriber and Synchronizer for ChannelInfo messages -// using ChannelInfoSubscriber = message_filters::Subscriber; -// using ChannelInfoSubscriberPtr = std::shared_ptr; -// using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime; -// using CameraChannelSync = message_filters::Synchronizer; -// using CameraChannelSyncPtr = std::shared_ptr; - -// // Subscriber and Synchronizer for Pointcloud messages -// using PointCloudSubscriber = message_filters::Subscriber; -// using PointCloudSubscriberPtr = std::shared_ptr; -// using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; -// using PointCloudSync = message_filters::Synchronizer; -// using PointCloudSyncPtr = std::shared_ptr; - -// private: -// void readParameters(); +namespace elevation_mapping_cupy { + +class ElevationMappingNode : public rclcpp::Node { + public: + ElevationMappingNode(); + using RowMatrixXd = Eigen::Matrix; + using ColMatrixXf = Eigen::Matrix; + + // using ImageSubscriber = image_transport::SubscriberFilter; + using ImageSubscriber = message_filters::Subscriber; + using ImageSubscriberPtr = std::shared_ptr; + + // Subscriber and Synchronizer for CameraInfo messages + using CameraInfoSubscriber = message_filters::Subscriber; + using CameraInfoSubscriberPtr = std::shared_ptr; + using CameraPolicy = message_filters::sync_policies::ApproximateTime; + using CameraSync = message_filters::Synchronizer; + using CameraSyncPtr = std::shared_ptr; + + // Subscriber and Synchronizer for ChannelInfo messages + using ChannelInfoSubscriber = message_filters::Subscriber; + using ChannelInfoSubscriberPtr = std::shared_ptr; + using CameraChannelPolicy = message_filters::sync_policies::ApproximateTime; + using CameraChannelSync = message_filters::Synchronizer; + using CameraChannelSyncPtr = std::shared_ptr; + + // Subscriber and Synchronizer for Pointcloud messages + using PointCloudSubscriber = message_filters::Subscriber; + using PointCloudSubscriberPtr = std::shared_ptr; + using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; + using PointCloudSync = message_filters::Synchronizer; + using PointCloudSyncPtr = std::shared_ptr; + + private: + void readParameters(); // void setupMapPublishers(); -// void pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key); -// void inputPointCloud(const sensor_msgs::PointCloud2& cloud, const std::vector& channels); + void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key); + void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector& channels); // void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector& channels); -// void imageCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::string& key); -// void imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); +// void imageCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const std::string& key); +// void imageChannelCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const elevation_map_msgs::msg::ChannelInfo::SharedPtr channel_info_msg); +void imageCallback(const std::shared_ptr& image_msg, const std::shared_ptr& camera_info_msg, const std::string& key); +void imageChannelCallback(const std::shared_ptr& image_msg, const std::shared_ptr& camera_info_msg, const std::shared_ptr& channel_info_msg); + // void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); // // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); // void publishAsPointCloud(const grid_map::GridMap& map) const; @@ -118,77 +125,86 @@ namespace py = pybind11; // void publishMapOfIndex(int index); // visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; - -// ros::NodeHandle nh_; -// image_transport::ImageTransport it_; -// std::vector pointcloudSubs_; -// std::vector imageSubs_; -// std::vector cameraInfoSubs_; -// std::vector channelInfoSubs_; -// std::vector cameraSyncs_; -// std::vector cameraChannelSyncs_; -// std::vector pointCloudSyncs_; -// std::vector mapPubs_; -// tf::TransformBroadcaster tfBroadcaster_; -// ros::Publisher alivePub_; -// ros::Publisher pointPub_; -// ros::Publisher normalPub_; -// ros::Publisher statisticsPub_; -// ros::ServiceServer rawSubmapService_; -// ros::ServiceServer clearMapService_; -// ros::ServiceServer clearMapWithInitializerService_; -// ros::ServiceServer initializeMapService_; -// ros::ServiceServer setPublishPointService_; -// ros::ServiceServer checkSafetyService_; -// ros::Timer updateVarianceTimer_; -// ros::Timer updateTimeTimer_; -// ros::Timer updatePoseTimer_; -// ros::Timer updateGridMapTimer_; -// ros::Timer publishStatisticsTimer_; -// ros::Time lastStatisticsPublishedTime_; -// tf::TransformListener transformListener_; -// ElevationMappingWrapper map_; -// std::string mapFrameId_; -// std::string correctedMapFrameId_; -// std::string baseFrameId_; - -// // map topics info -// std::vector> map_topics_; -// std::vector> map_layers_; -// std::vector> map_basic_layers_; -// std::set map_layers_all_; -// std::set map_layers_sync_; -// std::vector map_fps_; -// std::set map_fps_unique_; -// std::vector mapTimers_; -// std::map> channels_; - -// std::vector initialize_frame_id_; -// std::vector initialize_tf_offset_; -// std::string initializeMethod_; - -// Eigen::Vector3d lowpassPosition_; -// Eigen::Vector4d lowpassOrientation_; - -// std::mutex mapMutex_; // protects gridMap_ -// grid_map::GridMap gridMap_; -// std::atomic_bool isGridmapUpdated_; // needs to be atomic (read is not protected by mapMutex_) - -// std::mutex errorMutex_; // protects positionError_, and orientationError_ -// double positionError_; -// double orientationError_; - -// double positionAlpha_; -// double orientationAlpha_; - -// double recordableFps_; -// std::atomic_bool enablePointCloudPublishing_; -// bool enableNormalArrowPublishing_; -// bool enableDriftCorrectedTFPublishing_; -// bool useInitializerAtStart_; -// double initializeTfGridSize_; -// bool alwaysClearWithInitializer_; -// std::atomic_int pointCloudProcessCounter_; -// }; - -// } // namespace elevation_mapping_cupy + + rclcpp::Node::SharedPtr node_; + // image_transport::ImageTransport it_; + std::vector::SharedPtr> pointcloudSubs_; + std::vector imageSubs_; + std::vector cameraInfoSubs_; + std::vector channelInfoSubs_; + std::vector cameraSyncs_; + std::vector cameraChannelSyncs_; + std::vector pointCloudSyncs_; + std::vector::SharedPtr> mapPubs_; + + + std::shared_ptr tfBroadcaster_; + std::shared_ptr tfListener_; + std::shared_ptr tfBuffer_; + + + rclcpp::Publisher::SharedPtr alivePub_; + rclcpp::Publisher::SharedPtr pointPub_; + rclcpp::Publisher::SharedPtr normalPub_; + rclcpp::Publisher::SharedPtr statisticsPub_; + rclcpp::Service::SharedPtr rawSubmapService_; + rclcpp::Service::SharedPtr clearMapService_; + rclcpp::Service::SharedPtr clearMapWithInitializerService_; + rclcpp::Service::SharedPtr initializeMapService_; + rclcpp::Service::SharedPtr setPublishPointService_; + rclcpp::Service::SharedPtr checkSafetyService_; + rclcpp::TimerBase::SharedPtr updateVarianceTimer_; + rclcpp::TimerBase::SharedPtr updateTimeTimer_; + rclcpp::TimerBase::SharedPtr updatePoseTimer_; + rclcpp::TimerBase::SharedPtr updateGridMapTimer_; + rclcpp::TimerBase::SharedPtr publishStatisticsTimer_; + rclcpp::Time lastStatisticsPublishedTime_; + + + std::shared_ptr map_; + // ElevationMappingWrapper map_; + std::string mapFrameId_; + std::string correctedMapFrameId_; + std::string baseFrameId_; + + + // map topics info + std::vector> map_topics_; + std::vector> map_layers_; + std::vector> map_basic_layers_; + std::set map_layers_all_; + std::set map_layers_sync_; + std::vector map_fps_; + std::set map_fps_unique_; + std::vector mapTimers_; + std::map> channels_; + + std::vector initialize_frame_id_; + std::vector initialize_tf_offset_; + std::string initializeMethod_; + + Eigen::Vector3d lowpassPosition_; + Eigen::Vector4d lowpassOrientation_; + + std::mutex mapMutex_; // protects gridMap_ + grid_map::GridMap gridMap_; + std::atomic_bool isGridmapUpdated_; // needs to be atomic (read is not protected by mapMutex_) + + std::mutex errorMutex_; // protects positionError_, and orientationError_ + double positionError_; + double orientationError_; + + double positionAlpha_; + double orientationAlpha_; + + double recordableFps_; + std::atomic_bool enablePointCloudPublishing_; + bool enableNormalArrowPublishing_; + bool enableDriftCorrectedTFPublishing_; + bool useInitializerAtStart_; + double initializeTfGridSize_; + bool alwaysClearWithInitializer_; + std::atomic_int pointCloudProcessCounter_; +}; + +} // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index 92370e3c..0623d0f6 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -43,9 +43,9 @@ class ElevationMappingWrapper { using RowMatrixXf = Eigen::Matrix; using ColMatrixXf = Eigen::Matrix; - ElevationMappingWrapper(); + ElevationMappingWrapper(rclcpp::Node::SharedPtr node); - void initialize(rclcpp::Node::SharedPtr node); + void initialize(); void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); @@ -63,9 +63,11 @@ class ElevationMappingWrapper { double get_additive_mean_error(); void initializeWithPoints(std::vector& points, std::string method); void addNormalColorLayer(grid_map::GridMap& map); - - private: - void setParameters(rclcpp::Node::SharedPtr node); + + private: + rclcpp::Node::SharedPtr node_; + py::object setParameters(); + py::object map_; py::object param_; double resolution_; diff --git a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch deleted file mode 100644 index b0fb53c9..00000000 --- a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - \ No newline at end of file diff --git a/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py new file mode 100644 index 00000000..db110a12 --- /dev/null +++ b/elevation_mapping_cupy/launch/elevation_mapping_cupy.launch.py @@ -0,0 +1,28 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node +import launch_ros.actions + +def generate_launch_description(): + core_param_file = os.path.join( + get_package_share_directory('elevation_mapping_cupy'), + 'config', + 'core', + 'core_param.yaml' + ) + + + return LaunchDescription([ + launch_ros.actions.SetParameter(name='use_sim_time', value=True), + Node( + package='elevation_mapping_cupy', + executable='elevation_mapping_node', + name='elevation_mapping', + output='screen', + parameters=[core_param_file], + # arguments=['--ros-args', '--log-level', 'DEBUG'] + ) + ]) \ No newline at end of file diff --git a/elevation_mapping_cupy/package.xml b/elevation_mapping_cupy/package.xml index 9cb9c1bb..c1395b61 100644 --- a/elevation_mapping_cupy/package.xml +++ b/elevation_mapping_cupy/package.xml @@ -9,11 +9,13 @@ MIT ament_cmake + ament_cmake_python rclcpp std_msgs std_srvs sensor_msgs geometry_msgs + message_filters grid_map_msgs elevation_map_msgs diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/__init__.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/__init__.py index a16e894d..88d6a666 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/__init__.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/__init__.py @@ -1,2 +1,4 @@ -from .parameter import Parameter -from .elevation_mapping import ElevationMap +# from .parameter import Parameter +# from .elevation_mapping import ElevationMap +# from .kernels import custom_image_kernels, custom_kernels, custom_semantic_kernels + diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py index b78aeb45..4d704739 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -15,6 +15,7 @@ ) from elevation_mapping_cupy.parameter import Parameter + from elevation_mapping_cupy.kernels import ( add_points_kernel, add_color_kernel, @@ -932,11 +933,12 @@ def initialize_map(self, points, method="cubic"): print(R, t) param = Parameter( use_chainer=False, - weight_file="../config/weights.dat", - plugin_config_file="../config/plugin_config.yaml", + weight_file="/home/hojin/new_elev/src/elevation_mapping_cupy/elevation_mapping_cupy/config/core/weights.dat", + plugin_config_file="/home/hojin/new_elev/src/elevation_mapping_cupy/elevation_mapping_cupy/config/core/plugin_config.yaml", ) param.additional_layers = ["rgb", "grass", "tree", "people"] - param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"] + # param.fusion_algorithms = ["color", "class_bayesian", "class_bayesian", "class_bayesian"] + param.fusion_algorithms = ["image_color", "pointcloud_color", "pointcloud_class_average"] param.update() elevation = ElevationMap(param) layers = [ diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py index c28823e9..94cc1e5c 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping_ros.py @@ -1,287 +1,322 @@ -from elevation_mapping_cupy import ElevationMap -from elevation_mapping_cupy import Parameter - -# General -import os -import numpy as np - -# ROS -import rospy -import ros_numpy -from tf.transformations import quaternion_matrix -import tf2_ros -import rospkg -import message_filters -from cv_bridge import CvBridge - -from sensor_msgs.msg import PointCloud2, Image, CameraInfo -from grid_map_msgs.msg import GridMap -from std_msgs.msg import Float32MultiArray -from std_msgs.msg import MultiArrayLayout as MAL -from std_msgs.msg import MultiArrayDimension as MAD - -import numpy as np -from functools import partial - -PDC_DATATYPE = { - "1": np.int8, - "2": np.uint8, - "3": np.int16, - "4": np.uint16, - "5": np.int32, - "6": np.uint32, - "7": np.float32, - "8": np.float64, -} - - -class ElevationMapWrapper: - def __init__(self): - rospack = rospkg.RosPack() - self.root = rospack.get_path("elevation_mapping_cupy") - weight_file = os.path.join(self.root, "config/core/weights.dat") - plugin_config_file = os.path.join(self.root, "config/core/plugin_config.yaml") - self.param = Parameter(use_chainer=False, weight_file=weight_file, plugin_config_file=plugin_config_file) - - self.node_name = "elevation_mapping" - - # ROS - self.initalize_ros() - self.param.subscriber_cfg = self.subscribers - - self.initalize_elevation_mapping() - self.register_subscribers() - self.register_publishers() - self.register_timers() - - self._last_t = None - - def initalize_elevation_mapping(self): - self.param.update() - # TODO add statistics across processed topics - self._pointcloud_process_counter = 0 - self._image_process_counter = 0 - self._map = ElevationMap(self.param) - self._map_data = np.zeros((self._map.cell_n - 2, self._map.cell_n - 2), dtype=np.float32) - self._map_q = None - self._map_t = None - - def initalize_ros(self): - rospy.init_node(self.node_name, anonymous=False) - self._tf_buffer = tf2_ros.Buffer() - self._listener = tf2_ros.TransformListener(self._tf_buffer) - self.get_ros_params() - - def register_subscribers(self): - # check if CV bridge is needed - for config in self.param.subscriber_cfg.values(): - if config["data_type"] == "image": - self.cv_bridge = CvBridge() - break - - pointcloud_subs = {} - image_subs = {} - for key, config in self.subscribers.items(): - if config["data_type"] == "image": - camera_sub = message_filters.Subscriber(config["topic_name_camera"], Image) - camera_info_sub = message_filters.Subscriber(config["topic_name_camera_info"], CameraInfo) - image_subs[key] = message_filters.ApproximateTimeSynchronizer( - [camera_sub, camera_info_sub], queue_size=10, slop=0.5 - ) - image_subs[key].registerCallback(self.image_callback, key) - - elif config["data_type"] == "pointcloud": - pointcloud_subs[key] = rospy.Subscriber( - config["topic_name"], PointCloud2, self.pointcloud_callback, key - ) - - def register_publishers(self): - self._publishers = {} - self._publishers_timers = [] - for k, v in self.publishers.items(): - self._publishers[k] = rospy.Publisher(f"/{self.node_name}/{k}", GridMap, queue_size=10) - # partial(.) allows to pass a default argument to a callback - self._publishers_timers.append(rospy.Timer(rospy.Duration(1 / v["fps"]), partial(self.publish_map, k))) - - def publish_map(self, k, t): - print("publish_map") - if self._map_q is None: - return - gm = GridMap() - gm.info.header.frame_id = self.map_frame - gm.info.header.stamp = rospy.Time.now() - gm.info.header.seq = 0 - gm.info.resolution = self._map.resolution - gm.info.length_x = self._map.map_length - gm.info.length_y = self._map.map_length - gm.info.pose.position.x = self._map_t.x - gm.info.pose.position.y = self._map_t.y - gm.info.pose.position.z = 0.0 - gm.info.pose.orientation.w = 1.0 # self._map_q.w - gm.info.pose.orientation.x = 0.0 # self._map_q.x - gm.info.pose.orientation.y = 0.0 # self._map_q.y - gm.info.pose.orientation.z = 0.0 # self._map_q.z - gm.layers = [] - gm.basic_layers = self.publishers[k]["basic_layers"] - for i, layer in enumerate(self.publishers[k]["layers"]): - gm.layers.append(layer) - try: - self._map.get_map_with_name_ref(layer, self._map_data) - data = self._map_data.copy() - arr = Float32MultiArray() - arr.layout = MAL() - N = self._map_data.shape[0] - arr.layout.dim.append(MAD(label="column_index", size=N, stride=int(N * N))) - arr.layout.dim.append(MAD(label="row_index", size=N, stride=N)) - arr.data = tuple(np.ascontiguousarray(data.T).reshape(-1)) - gm.data.append(arr) - except: - if layer in gm.basic_layers: - print("Error: Missed Layer in basic layers") - - gm.outer_start_index = 0 - gm.inner_start_index = 0 - - self._publishers[k].publish(gm) - - def register_timers(self): - self.timer_variance = rospy.Timer(rospy.Duration(1 / self.update_variance_fps), self.update_variance) - self.timer_pose = rospy.Timer(rospy.Duration(1 / self.update_pose_fps), self.update_pose) - self.timer_time = rospy.Timer(rospy.Duration(self.time_interval), self.update_time) - - def image_callback(self, camera_msg, camera_info_msg, sub_key): - # get pose of image - ti = rospy.Time(secs=camera_msg.header.stamp.secs, nsecs=camera_msg.header.stamp.nsecs) - self._last_t = ti - try: - transform = self._tf_buffer.lookup_transform( - camera_msg.header.frame_id, self.map_frame, ti, rospy.Duration(1.0) - ) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - print("Error: image_callback:", e) - return - - t = transform.transform.translation - t = np.array([t.x, t.y, t.z]) - q = transform.transform.rotation - R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] - - semantic_img = self.cv_bridge.imgmsg_to_cv2(camera_msg, desired_encoding="passthrough") - - if len(semantic_img.shape) != 2: - semantic_img = [semantic_img[:, :, k] for k in range(3)] - - else: - semantic_img = [semantic_img] - - K = np.array(camera_info_msg.K, dtype=np.float32).reshape(3, 3) - - assert np.all(np.array(camera_info_msg.D) == 0.0), "Undistortion not implemented" - D = np.array(camera_info_msg.D, dtype=np.float32).reshape(5, 1) +# # General +# import os +# import numpy as np +# import yaml + +# from elevation_mapping_cupy import ElevationMap, Parameter + + + +# # ROS2 +# import rclpy +# from rclpy.node import Node +# from tf_transformations import quaternion_matrix +# import tf2_ros +# from ament_index_python.packages import get_package_share_directory +# from cv_bridge import CvBridge + +# from sensor_msgs.msg import PointCloud2, Image, CameraInfo +# from grid_map_msgs.msg import GridMap +# from std_msgs.msg import Float32MultiArray, MultiArrayLayout as MAL, MultiArrayDimension as MAD + +# from functools import partial +# import message_filters + + +# PDC_DATATYPE = { +# "1": np.int8, +# "2": np.uint8, +# "3": np.int16, +# "4": np.uint16, +# "5": np.int32, +# "6": np.uint32, +# "7": np.float32, +# "8": np.float64, +# } + + +# class ElevationMapWrapper(Node): +# def __init__(self): +# super().__init__('elevation_mapping') +# self.node_name = "elevation_mapping" +# self.root = get_package_share_directory("elevation_mapping_cupy") +# weight_file = os.path.join(self.root, "config/core/weights.dat") +# plugin_config_file = os.path.join(self.root, "config/core/plugin_config.yaml") +# self.param = Parameter(use_chainer=False, weight_file=weight_file, plugin_config_file=plugin_config_file) - # process pointcloud - self._map.input_image(sub_key, semantic_img, R, t, K, D, camera_info_msg.height, camera_info_msg.width) - self._image_process_counter += 1 - - def pointcloud_callback(self, msg, sub_key): - channels = ["x", "y", "z"] + self.param.subscriber_cfg[sub_key]["channels"] - - points = ros_numpy.numpify(msg) - pts = np.empty((points.shape[0], 0)) - for ch in channels: - p = points[ch] - if len(p.shape) == 1: - p = p[:, None] - pts = np.append(pts, p, axis=1) - - # get pose of pointcloud - ti = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs) - self._last_t = ti - try: - transform = self._tf_buffer.lookup_transform(self.map_frame, msg.header.frame_id, ti, rospy.Duration(1.0)) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - print("Error: pointcloud_callback: ", e) - return - - t = transform.transform.translation - t = np.array([t.x, t.y, t.z]) - q = transform.transform.rotation - R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] - - # process pointcloud - self._map.input(pts, channels, R, t, 0, 0) - self._pointcloud_process_counter += 1 - print("Pointclouds processed: ", self._pointcloud_process_counter) - - def update_pose(self, t): - # get pose of base - # t = rospy.Time.now() - if self._last_t is None: - return - try: - transform = self._tf_buffer.lookup_transform( - self.map_frame, self.base_frame, self._last_t, rospy.Duration(1.0) - ) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - print("Error: update_pose error: ", e) - return - t = transform.transform.translation - trans = np.array([t.x, t.y, t.z]) - q = transform.transform.rotation - rot = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] - self._map.move_to(trans, rot) - - self._map_t = t - self._map_q = q - - def update_variance(self, t): - self._map.update_variance() - - def update_time(self, t): - self._map.update_time() - - def get_ros_params(self): - # TODO fix this here when later launching with launch-file - # This is currently {p} elevation_mapping") - typ = "sim" - para = os.path.join(self.root, f"config/{typ}_parameters.yaml") - sens = os.path.join(self.root, f"config/{typ}_sensor_parameter.yaml") - plugin = os.path.join(self.root, f"config/{typ}_plugin_config.yaml") - - os.system(f"rosparam delete /{self.node_name}") - os.system(f"rosparam load {para} elevation_mapping") - os.system(f"rosparam load {sens} elevation_mapping") - os.system(f"rosparam load {plugin} elevation_mapping") - - self.subscribers = rospy.get_param("~subscribers") - self.publishers = rospy.get_param("~publishers") - self.initialize_frame_id = rospy.get_param("~initialize_frame_id", "base") - self.initialize_tf_offset = rospy.get_param("~initialize_tf_offset", 0.0) - self.pose_topic = rospy.get_param("~pose_topic", "pose") - self.map_frame = rospy.get_param("~map_frame", "map") - self.base_frame = rospy.get_param("~base_frame", "base") - self.corrected_map_frame = rospy.get_param("~corrected_map_frame", "corrected_map") - self.initialize_method = rospy.get_param("~initialize_method", "cubic") - self.position_lowpass_alpha = rospy.get_param("~position_lowpass_alpha", 0.2) - self.orientation_lowpass_alpha = rospy.get_param("~orientation_lowpass_alpha", 0.2) - self.recordable_fps = rospy.get_param("~recordable_fps", 3.0) - self.update_variance_fps = rospy.get_param("~update_variance_fps", 1.0) - self.time_interval = rospy.get_param("~time_interval", 0.1) - self.update_pose_fps = rospy.get_param("~update_pose_fps", 10.0) - self.initialize_tf_grid_size = rospy.get_param("~initialize_tf_grid_size", 0.5) - self.map_acquire_fps = rospy.get_param("~map_acquire_fps", 5.0) - self.publish_statistics_fps = rospy.get_param("~publish_statistics_fps", 1.0) - self.enable_pointcloud_publishing = rospy.get_param("~enable_pointcloud_publishing", False) - self.enable_normal_arrow_publishing = rospy.get_param("~enable_normal_arrow_publishing", False) - self.enable_drift_corrected_TF_publishing = rospy.get_param("~enable_drift_corrected_TF_publishing", False) - self.use_initializer_at_start = rospy.get_param("~use_initializer_at_start", False) - - -if __name__ == "__main__": - emw = ElevationMapWrapper() - - while not rospy.is_shutdown(): - try: - rospy.spin() - except rospy.ROSInterruptException: - print("Error") +# # ROS2 +# self.initialize_ros() +# self.param.subscriber_cfg = self.subscribers + +# self.initialize_elevation_mapping() +# # self.register_subscribers() +# # self.register_publishers() +# # self.register_timers() + +# self._last_t = None + + + + +# # # ROS +# # self.initalize_ros() +# # self.param.subscriber_cfg = self.subscribers + + +# def initialize_ros(self): +# self._tf_buffer = tf2_ros.Buffer() +# self._listener = tf2_ros.TransformListener(self._tf_buffer, self) +# self.get_ros_params() + + +# def initialize_elevation_mapping(self): +# self.param.update() +# # TODO add statistics across processed topics +# self._pointcloud_process_counter = 0 +# self._image_process_counter = 0 +# self._map = ElevationMap(self.param) +# self._map_data = np.zeros((self._map.cell_n - 2, self._map.cell_n - 2), dtype=np.float32) +# self._map_q = None +# self._map_t = None + + + + +# # def register_subscribers(self): +# # # check if CV bridge is needed +# # for config in self.param.subscriber_cfg.values(): +# # if config["data_type"] == "image": +# # self.cv_bridge = CvBridge() +# # break + +# # pointcloud_subs = {} +# # image_subs = {} +# # for key, config in self.subscribers.items(): +# # if config["data_type"] == "image": +# # camera_sub = message_filters.Subscriber(config["topic_name_camera"], Image) +# # camera_info_sub = message_filters.Subscriber(config["topic_name_camera_info"], CameraInfo) +# # image_subs[key] = message_filters.ApproximateTimeSynchronizer( +# # [camera_sub, camera_info_sub], queue_size=10, slop=0.5 +# # ) +# # image_subs[key].registerCallback(self.image_callback, key) + +# # elif config["data_type"] == "pointcloud": +# # pointcloud_subs[key] = rospy.Subscriber( +# # config["topic_name"], PointCloud2, self.pointcloud_callback, key +# # ) + +# # def register_publishers(self): +# # self._publishers = {} +# # self._publishers_timers = [] +# # for k, v in self.publishers.items(): +# # self._publishers[k] = rospy.Publisher(f"/{self.node_name}/{k}", GridMap, queue_size=10) +# # # partial(.) allows to pass a default argument to a callback +# # self._publishers_timers.append(rospy.Timer(rospy.Duration(1 / v["fps"]), partial(self.publish_map, k))) + +# # def publish_map(self, k, t): +# # print("publish_map") +# # if self._map_q is None: +# # return +# # gm = GridMap() +# # gm.info.header.frame_id = self.map_frame +# # gm.info.header.stamp = rospy.Time.now() +# # gm.info.header.seq = 0 +# # gm.info.resolution = self._map.resolution +# # gm.info.length_x = self._map.map_length +# # gm.info.length_y = self._map.map_length +# # gm.info.pose.position.x = self._map_t.x +# # gm.info.pose.position.y = self._map_t.y +# # gm.info.pose.position.z = 0.0 +# # gm.info.pose.orientation.w = 1.0 # self._map_q.w +# # gm.info.pose.orientation.x = 0.0 # self._map_q.x +# # gm.info.pose.orientation.y = 0.0 # self._map_q.y +# # gm.info.pose.orientation.z = 0.0 # self._map_q.z +# # gm.layers = [] +# # gm.basic_layers = self.publishers[k]["basic_layers"] +# # for i, layer in enumerate(self.publishers[k]["layers"]): +# # gm.layers.append(layer) +# # try: +# # self._map.get_map_with_name_ref(layer, self._map_data) +# # data = self._map_data.copy() +# # arr = Float32MultiArray() +# # arr.layout = MAL() +# # N = self._map_data.shape[0] +# # arr.layout.dim.append(MAD(label="column_index", size=N, stride=int(N * N))) +# # arr.layout.dim.append(MAD(label="row_index", size=N, stride=N)) +# # arr.data = tuple(np.ascontiguousarray(data.T).reshape(-1)) +# # gm.data.append(arr) +# # except: +# # if layer in gm.basic_layers: +# # print("Error: Missed Layer in basic layers") + +# # gm.outer_start_index = 0 +# # gm.inner_start_index = 0 + +# # self._publishers[k].publish(gm) + +# # def register_timers(self): +# # self.timer_variance = rospy.Timer(rospy.Duration(1 / self.update_variance_fps), self.update_variance) +# # self.timer_pose = rospy.Timer(rospy.Duration(1 / self.update_pose_fps), self.update_pose) +# # self.timer_time = rospy.Timer(rospy.Duration(self.time_interval), self.update_time) + +# # def image_callback(self, camera_msg, camera_info_msg, sub_key): +# # # get pose of image +# # ti = rospy.Time(secs=camera_msg.header.stamp.secs, nsecs=camera_msg.header.stamp.nsecs) +# # self._last_t = ti +# # try: +# # transform = self._tf_buffer.lookup_transform( +# # camera_msg.header.frame_id, self.map_frame, ti, rospy.Duration(1.0) +# # ) +# # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: +# # print("Error: image_callback:", e) +# # return + +# # t = transform.transform.translation +# # t = np.array([t.x, t.y, t.z]) +# # q = transform.transform.rotation +# # R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] + +# # semantic_img = self.cv_bridge.imgmsg_to_cv2(camera_msg, desired_encoding="passthrough") + +# # if len(semantic_img.shape) != 2: +# # semantic_img = [semantic_img[:, :, k] for k in range(3)] + +# # else: +# # semantic_img = [semantic_img] + +# # K = np.array(camera_info_msg.K, dtype=np.float32).reshape(3, 3) + +# # assert np.all(np.array(camera_info_msg.D) == 0.0), "Undistortion not implemented" +# # D = np.array(camera_info_msg.D, dtype=np.float32).reshape(5, 1) + +# # # process pointcloud +# # self._map.input_image(sub_key, semantic_img, R, t, K, D, camera_info_msg.height, camera_info_msg.width) +# # self._image_process_counter += 1 + +# # def pointcloud_callback(self, msg, sub_key): +# # channels = ["x", "y", "z"] + self.param.subscriber_cfg[sub_key]["channels"] + +# # points = ros_numpy.numpify(msg) +# # pts = np.empty((points.shape[0], 0)) +# # for ch in channels: +# # p = points[ch] +# # if len(p.shape) == 1: +# # p = p[:, None] +# # pts = np.append(pts, p, axis=1) + +# # # get pose of pointcloud +# # ti = rospy.Time(secs=msg.header.stamp.secs, nsecs=msg.header.stamp.nsecs) +# # self._last_t = ti +# # try: +# # transform = self._tf_buffer.lookup_transform(self.map_frame, msg.header.frame_id, ti, rospy.Duration(1.0)) +# # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: +# # print("Error: pointcloud_callback: ", e) +# # return + +# # t = transform.transform.translation +# # t = np.array([t.x, t.y, t.z]) +# # q = transform.transform.rotation +# # R = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] + +# # # process pointcloud +# # self._map.input(pts, channels, R, t, 0, 0) +# # self._pointcloud_process_counter += 1 +# # print("Pointclouds processed: ", self._pointcloud_process_counter) + +# # def update_pose(self, t): +# # # get pose of base +# # # t = rospy.Time.now() +# # if self._last_t is None: +# # return +# # try: +# # transform = self._tf_buffer.lookup_transform( +# # self.map_frame, self.base_frame, self._last_t, rospy.Duration(1.0) +# # ) +# # except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: +# # print("Error: update_pose error: ", e) +# # return +# # t = transform.transform.translation +# # trans = np.array([t.x, t.y, t.z]) +# # q = transform.transform.rotation +# # rot = quaternion_matrix([q.x, q.y, q.z, q.w])[:3, :3] +# # self._map.move_to(trans, rot) + +# # self._map_t = t +# # self._map_q = q + +# # def update_variance(self, t): +# # self._map.update_variance() + +# # def update_time(self, t): +# # self._map.update_time() + + + +# def declare_parameters_from_yaml(self, yaml_file): +# def declare_nested_parameters(parent_name, param_dict): +# for sub_name, sub_value in param_dict.items(): +# full_param_name = f"{parent_name}.{sub_name}" + +# if isinstance(sub_value, dict): +# declare_nested_parameters(full_param_name, sub_value) +# else: +# if "elevation_mapping.ros__parameters" in full_param_name: +# full_param_name = full_param_name.replace("elevation_mapping.ros__parameters.", "") +# self.declare_parameter(full_param_name, sub_value) + +# with open(yaml_file, 'r') as file: +# params = yaml.safe_load(file) + +# # Set parameters in the node +# for param_name, param_value in params.items(): +# if isinstance(param_value, dict): +# # For nested dictionaries, set each nested parameter individually +# declare_nested_parameters(param_name, param_value) +# else: +# # For top-level parameters +# if "elevation_mapping.ros__parameters" in param_name: +# param_name = param_name.replace("elevation_mapping.ros__parameters.", "") +# self.declare_parameter(param_name, param_value) + + + + +# def get_ros_params(self): +# # TODO fix this here when later launching with launch-file +# # This is currently {p} elevation_mapping") +# param_file = os.path.join(self.root, f"config/core/core_param.yaml") +# self.declare_parameters_from_yaml(param_file) + +# # os.system(f"rosparam delete /{self.node_name}") +# # os.system(f"rosparam load {para} elevation_mapping") +# # self.subscribers = self.get_parameter("subscribers").get_parameter_value().string_value +# # self.publishers = self.get_parameter("publishers").get_parameter_value().string_value +# self.initialize_frame_id = self.get_parameter("initialize_frame_id").get_parameter_value().string_array_value +# self.initialize_tf_offset = self.get_parameter("initialize_tf_offset").get_parameter_value().double_value +# self.pose_topic = self.get_parameter("pose_topic").get_parameter_value().string_value +# self.map_frame = self.get_parameter("map_frame").get_parameter_value().string_value +# self.base_frame = self.get_parameter("base_frame").get_parameter_value().string_value +# self.corrected_map_frame = self.get_parameter("corrected_map_frame").get_parameter_value().string_value +# self.initialize_method = self.get_parameter("initialize_method").get_parameter_value().string_value +# self.position_lowpass_alpha = self.get_parameter("position_lowpass_alpha").get_parameter_value().double_value +# self.orientation_lowpass_alpha = self.get_parameter("orientation_lowpass_alpha").get_parameter_value().double_value +# self.update_variance_fps = self.get_parameter("update_variance_fps").get_parameter_value().double_value +# self.time_interval = self.get_parameter("time_interval").get_parameter_value().double_value +# self.update_pose_fps = self.get_parameter("update_pose_fps").get_parameter_value().double_value +# self.initialize_tf_grid_size = self.get_parameter("initialize_tf_grid_size").get_parameter_value().double_value +# self.map_acquire_fps = self.get_parameter("map_acquire_fps").get_parameter_value().double_value +# self.publish_statistics_fps = self.get_parameter("publish_statistics_fps").get_parameter_value().double_value +# self.enable_pointcloud_publishing = self.get_parameter("enable_pointcloud_publishing").get_parameter_value().bool_value +# self.enable_normal_arrow_publishing = self.get_parameter("enable_normal_arrow_publishing").get_parameter_value().bool_value +# self.enable_drift_corrected_TF_publishing = self.get_parameter("enable_drift_corrected_TF_publishing").get_parameter_value().bool_value +# self.use_initializer_at_start = self.get_parameter("use_initializer_at_start").get_parameter_value().bool_value + + +# def main(args=None): +# rclpy.init(args=args) +# node = ElevationMapWrapper() +# rclpy.spin(node) +# rclpy.shutdown() + +# if __name__ == '__main__': +# main() \ No newline at end of file diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/kk.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/kk.py new file mode 100644 index 00000000..27562dda --- /dev/null +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/kernels/kk.py @@ -0,0 +1,1290 @@ +# +# Copyright (c) 2023, Takahiro Miki. All rights reserved. +# Licensed under the MIT license. See LICENSE file in the project root for details. +# +import cupy as cp +import string + + +def sum_kernel( + resolution, width, height, +): + """Sums the semantic values of the classes for the exponentiala verage or for the average. + + Args: + resolution: + width: + height: + + Returns: + + """ + # input the list of layers, amount of channels can slo be input through kernel + sum_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map, raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, map_lay[layer])], feat); + } + } + """ + ).substitute(), + name="sum_kernel", + ) + return sum_kernel + + +def sum_compact_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_compact_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + U feat = p[id * pcl_channels[0] + pcl_chan[layer]]; + atomicAdd(&newmap[get_map_idx(idx, layer)], feat); + } + } + """ + ).substitute(), + name="sum_compact_kernel", + ) + return sum_compact_kernel + + +def sum_max_kernel( + resolution, width, height, +): + # input the list of layers, amount of channels can slo be input through kernel + sum_max_kernel = cp.ElementwiseKernel( + in_params="raw U p, raw U max_pt, raw T max_id, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params=" raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(resolution=resolution, width=width, height=height), + operation=string.Template( + """ + U idx = p[i * pcl_channels[0]]; + U valid = p[i * pcl_channels[0] + 1]; + U inside = p[i * pcl_channels[0] + 2]; + if (valid) { + if (inside) { + // for every max value + for ( W it=0;it=theta_max){ + arg_max = map_lay[layer]; + theta_max = theta; + } + atomicAdd(&newmap[get_map_idx(idx, arg_max)], theta_max); + } + } + """ + ).substitute(), + name="alpha_kernel", + ) + return alpha_kernel + + +def average_kernel( + width, height, +): + average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = feat; + } + """ + ).substitute(), + name="average_map_kernel", + ) + return average_kernel + + +def bayesian_inference_kernel( + width, height, +): + bayesian_inference_kernel = cp.ElementwiseKernel( + in_params=" raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U newmap, raw U sum_mean, raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U feat_ml = sum_mean[get_map_idx(id, layer)]/cnt; + U feat_old = map[get_map_idx(id, map_lay[layer])]; + U sigma_old = newmap[get_map_idx(id, map_lay[layer])]; + U sigma = 1.0; + U feat_new = sigma*feat_old /(cnt*sigma_old + sigma) +cnt*sigma_old *feat_ml /(cnt*sigma_old+sigma); + U sigma_new = sigma*sigma_old /(cnt*sigma_old +sigma); + map[get_map_idx(id, map_lay[layer])] = feat_new; + newmap[get_map_idx(id, map_lay[layer])] = sigma_new; + } + """ + ).substitute(), + name="bayesian_inference_kernel", + ) + return bayesian_inference_kernel + + +def class_average_kernel( + width, height, alpha, +): + class_average_kernel = cp.ElementwiseKernel( + in_params="raw V newmap, raw W pcl_chan, raw W map_lay, raw W pcl_channels, raw U new_elmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U cnt = new_elmap[get_map_idx(id, 2)]; + if (cnt>0){ + U prev_val = map[get_map_idx(id, map_lay[layer])]; + if (prev_val==0){ + U val = newmap[get_map_idx(id, map_lay[layer])]/(1*cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + else{ + U val = ${alpha} *prev_val + (1-${alpha}) * newmap[get_map_idx(id, map_lay[layer])]/(cnt); + map[get_map_idx(id, map_lay[layer])] = val; + } + } + """ + ).substitute(alpha=alpha,), + name="class_average_kernel", + ) + return class_average_kernel + + +def add_color_kernel( + width, height, +): + add_color_kernel = cp.ElementwiseKernel( + in_params="raw T p, raw U R, raw U t, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw V color_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = ( color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + U idx = p[id * pcl_channels[0]]; + U valid = p[id * pcl_channels[0] + 1]; + U inside = p[id * pcl_channels[0] + 2]; + if (valid && inside){ + unsigned int color = __float_as_uint(p[id * pcl_channels[0] + pcl_chan[layer]]); + atomicAdd(&color_map[get_map_idx(idx, layer*3)], get_r(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3+1)], get_g(color)); + atomicAdd(&color_map[get_map_idx(idx, layer*3 + 2)], get_b(color)); + atomicAdd(&color_map[get_map_idx(idx, pcl_channels[1]*3)], 1); + } + """ + ).substitute(width=width), + name="add_color_kernel", + ) + return add_color_kernel + + +def color_average_kernel( + width, height, +): + color_average_kernel = cp.ElementwiseKernel( + in_params="raw V color_map, raw W pcl_chan, raw W map_lay, raw W pcl_channels", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ unsigned int get_r(unsigned int color){ + unsigned int red = 0xFF0000; + unsigned int reds = (color & red) >> 16; + return reds; + } + __device__ unsigned int get_g(unsigned int color){ + unsigned int green = 0xFF00; + unsigned int greens = (color & green) >> 8; + return greens; + } + __device__ unsigned int get_b(unsigned int color){ + unsigned int blue = 0xFF; + unsigned int blues = (color & blue); + return blues; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U id = floorf(i/pcl_channels[1]); + int layer = i % pcl_channels[1]; + unsigned int cnt = color_map[get_map_idx(id, pcl_channels[1]*3)]; + if (cnt>0){ + // U prev_color = map[get_map_idx(id, map_lay[layer])]; + unsigned int r = color_map[get_map_idx(id, layer*3)]/(1*cnt); + unsigned int g = color_map[get_map_idx(id, layer*3+1)]/(1*cnt); + unsigned int b = color_map[get_map_idx(id, layer*3+2)]/(1*cnt); + //if (prev_color>=0){ + // unsigned int prev_r = get_r(prev_color); + // unsigned int prev_g = get_g(prev_color); + // unsigned int prev_b = get_b(prev_color); + // unsigned int r = prev_r/2 + color_map[get_map_idx(i, layer*3)]/(2*cnt); + // unsigned int g = prev_g/2 + color_map[get_map_idx(i, layer*3+1)]/(2*cnt); + // unsigned int b = prev_b/2 + color_map[get_map_idx(i, layer*3+2)]/(2*cnt); + //} + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + map[get_map_idx(id, map_lay[layer])] = rgb_; + } + """ + ).substitute(), + name="color_average_kernel", + ) + return color_average_kernel + + + +def map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + util_preamble = string.Template( + """ + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ int get_x_idx(float16 x, float16 center) { + int i = (x - center) / ${resolution} + 0.5 * ${width}; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + int i = (y - center) / ${resolution} + 0.5 * ${height}; + return i; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x == 0 || idx_x == ${width} - 1) { + return false; + } + if (idx_y == 0 || idx_y == ${height} - 1) { + return false; + } + return true; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ float transform_p(float16 x, float16 y, float16 z, + float16 r0, float16 r1, float16 r2, float16 t) { + return r0 * x + r1 * y + r2 * z + t; + } + __device__ float z_noise(float16 z){ + return ${sensor_noise_factor} * z * z; + } + + __device__ float point_sensor_distance(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = (x - sx) * (x - sx) + (y - sy) * (y - sy) + (z - sz) * (z - sz); + return d; + } + + __device__ bool is_valid(float16 x, float16 y, float16 z, + float16 sx, float16 sy, float16 sz) { + float d = point_sensor_distance(x, y, z, sx, sy, sz); + float dxy = max(sqrt(x * x + y * y) - ${ramped_height_range_b}, 0.0); + if (d < ${min_valid_distance} * ${min_valid_distance}) { + return false; + } + else if (z - sz > dxy * ${ramped_height_range_a} + ${ramped_height_range_c} || z - sz > ${max_height_range}) { + return false; + } + else { + return true; + } + } + + __device__ float ray_vector(float16 tx, float16 ty, float16 tz, + float16 px, float16 py, float16 pz, + float16& rx, float16& ry, float16& rz){ + float16 vx = px - tx; + float16 vy = py - ty; + float16 vz = pz - tz; + float16 norm = sqrt(vx * vx + vy * vy + vz * vz); + if (norm > 0) { + rx = vx / norm; + ry = vy / norm; + rz = vz / norm; + } + else { + rx = 0; + ry = 0; + rz = 0; + } + return norm; + } + + __device__ float inner_product(float16 x1, float16 y1, float16 z1, + float16 x2, float16 y2, float16 z2) { + + float product = (x1 * x2 + y1 * y2 + z1 * z2); + return product; + } + + """ + ).substitute( + resolution=resolution, + width=width, + height=height, + sensor_noise_factor=sensor_noise_factor, + min_valid_distance=min_valid_distance, + max_height_range=max_height_range, + ramped_height_range_a=ramped_height_range_a, + ramped_height_range_b=ramped_height_range_b, + ramped_height_range_c=ramped_height_range_c, + ) + return util_preamble + + +def add_points_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + wall_num_thresh, + max_ray_length, + cleanup_step, + min_valid_distance, + max_height_range, + cleanup_cos_thresh, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + enable_edge_shaped=True, + enable_visibility_cleanup=True, +): + add_points_kernel = cp.ElementwiseKernel( + in_params="raw U center_x, raw U center_y, raw U R, raw U t, raw U norm_map", + out_params="raw U p, raw U map, raw T newmap", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (is_valid(x, y, z, t[0], t[1], t[2])) { + if (is_inside(idx)) { + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U num_points = newmap[get_map_idx(idx, 4)]; + if (abs(map_h - z) > (map_v * ${mahalanobis_thresh})) { + atomicAdd(&map[get_map_idx(idx, 1)], ${outlier_variance}); + } + else { + if (${enable_edge_shaped} && (num_points > ${wall_num_thresh}) && (z < map_h - map_v * ${mahalanobis_thresh} / num_points)) { + // continue; + } + else { + T new_h = (map_h * v + z * map_v) / (map_v + v); + T new_v = (map_v * v) / (map_v + v); + atomicAdd(&newmap[get_map_idx(idx, 0)], new_h); + atomicAdd(&newmap[get_map_idx(idx, 1)], new_v); + atomicAdd(&newmap[get_map_idx(idx, 2)], 1.0); + // is Valid + map[get_map_idx(idx, 2)] = 1; + // Time layer + map[get_map_idx(idx, 4)] = 0.0; + // Upper bound + map[get_map_idx(idx, 5)] = new_h; + map[get_map_idx(idx, 6)] = 0.0; + } + // visibility cleanup + } + } + } + if (${enable_visibility_cleanup}) { + float16 ray_x, ray_y, ray_z; + float16 ray_length = ray_vector(t[0], t[1], t[2], x, y, z, ray_x, ray_y, ray_z); + ray_length = min(ray_length, (float16)${max_ray_length}); + int last_nidx = -1; + for (float16 s=${ray_step}; s < ray_length; s+=${ray_step}) { + // iterate through ray + U nx = t[0] + ray_x * s; + U ny = t[1] + ray_y * s; + U nz = t[2] + ray_z * s; + int nidx = get_idx(nx, ny, center_x[0], center_y[0]); + if (last_nidx == nidx) {continue;} // Skip if we're still in the same cell + else {last_nidx = nidx;} + if (!is_inside(nidx)) {continue;} + + U nmap_h = map[get_map_idx(nidx, 0)]; + U nmap_v = map[get_map_idx(nidx, 1)]; + U nmap_valid = map[get_map_idx(nidx, 2)]; + // traversability + U nmap_trav = map[get_map_idx(nidx, 3)]; + // Time layer + U non_updated_t = map[get_map_idx(nidx, 4)]; + // upper bound + U nmap_upper = map[get_map_idx(nidx, 5)]; + U nmap_is_upper = map[get_map_idx(nidx, 6)]; + + // If point is close or is farther away than ray length, skip. + float16 d = (x - nx) * (x - nx) + (y - ny) * (y - ny) + (z - nz) * (z - nz); + if (d < 0.1 || !is_valid(x, y, z, t[0], t[1], t[2])) {continue;} + + // If invalid, do upper bound check, then skip + if (nmap_valid < 0.5) { + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + continue; + } + // If updated recently, skip + if (non_updated_t < 0.5) {continue;} + + if (nmap_h > nz + 0.01 - min(nmap_v, 1.0) * 0.05) { + // If ray and norm is vertical, skip + U norm_x = norm_map[get_map_idx(nidx, 0)]; + U norm_y = norm_map[get_map_idx(nidx, 1)]; + U norm_z = norm_map[get_map_idx(nidx, 2)]; + float product = inner_product(ray_x, ray_y, ray_z, norm_x, norm_y, norm_z); + if (fabs(product) < ${cleanup_cos_thresh}) {continue;} + U num_points = newmap[get_map_idx(nidx, 3)]; + if (num_points > ${wall_num_thresh} && non_updated_t < 1.0) {continue;} + + // Finally, this cell is penetrated by the ray. + atomicAdd(&map[get_map_idx(nidx, 2)], -${cleanup_step}/(ray_length / ${max_ray_length})); + atomicAdd(&map[get_map_idx(nidx, 1)], ${outlier_variance}); + // Do upper bound check. + if (nz < nmap_upper || nmap_is_upper < 0.5) { + map[get_map_idx(nidx, 5)] = nz; + map[get_map_idx(nidx, 6)] = 1.0f; + } + } + } + } + p[i * 3]= idx; + p[i * 3 + 1] = is_valid(x, y, z, t[0], t[1], t[2]); + p[i * 3 + 2] = is_inside(idx); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + wall_num_thresh=wall_num_thresh, + ray_step=resolution / 2 ** 0.5, + max_ray_length=max_ray_length, + cleanup_step=cleanup_step, + cleanup_cos_thresh=cleanup_cos_thresh, + enable_edge_shaped=int(enable_edge_shaped), + enable_visibility_cleanup=int(enable_visibility_cleanup), + ), + name="add_points_kernel", + ) + return add_points_kernel + + +def error_counting_kernel( + resolution, + width, + height, + sensor_noise_factor, + mahalanobis_thresh, + outlier_variance, + traversability_inlier, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, +): + error_counting_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U p, raw U center_x, raw U center_y, raw U R, raw U t", + out_params="raw U newmap, raw T error, raw T error_cnt", + preamble=map_utils( + resolution, + width, + height, + sensor_noise_factor, + min_valid_distance, + max_height_range, + ramped_height_range_a, + ramped_height_range_b, + ramped_height_range_c, + ), + operation=string.Template( + """ + U rx = p[i * 3]; + U ry = p[i * 3 + 1]; + U rz = p[i * 3 + 2]; + U x = transform_p(rx, ry, rz, R[0], R[1], R[2], t[0]); + U y = transform_p(rx, ry, rz, R[3], R[4], R[5], t[1]); + U z = transform_p(rx, ry, rz, R[6], R[7], R[8], t[2]); + U v = z_noise(rz); + // if (!is_valid(z, t[2])) {return;} + if (!is_valid(x, y, z, t[0], t[1], t[2])) {return;} + // if ((x - t[0]) * (x - t[0]) + (y - t[1]) * (y - t[1]) + (z - t[2]) * (z - t[2]) < 0.5) {return;} + int idx = get_idx(x, y, center_x[0], center_y[0]); + if (!is_inside(idx)) { + return; + } + U map_h = map[get_map_idx(idx, 0)]; + U map_v = map[get_map_idx(idx, 1)]; + U map_valid = map[get_map_idx(idx, 2)]; + U map_t = map[get_map_idx(idx, 3)]; + if (map_valid > 0.5 && (abs(map_h - z) < (map_v * ${mahalanobis_thresh})) + && map_v < ${outlier_variance} / 2.0 + && map_t > ${traversability_inlier}) { + T e = z - map_h; + atomicAdd(&error[0], e); + atomicAdd(&error_cnt[0], 1); + atomicAdd(&newmap[get_map_idx(idx, 3)], 1.0); + } + atomicAdd(&newmap[get_map_idx(idx, 4)], 1.0); + """ + ).substitute( + mahalanobis_thresh=mahalanobis_thresh, + outlier_variance=outlier_variance, + traversability_inlier=traversability_inlier, + ), + name="error_counting_kernel", + ) + return error_counting_kernel + + +def average_map_kernel(width, height, max_variance, initial_variance): + average_map_kernel = cp.ElementwiseKernel( + in_params="raw U newmap", + out_params="raw U map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U v = map[get_map_idx(i, 1)]; + U valid = map[get_map_idx(i, 2)]; + U new_h = newmap[get_map_idx(i, 0)]; + U new_v = newmap[get_map_idx(i, 1)]; + U new_cnt = newmap[get_map_idx(i, 2)]; + if (new_cnt > 0) { + if (new_v / new_cnt > ${max_variance}) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + else { + map[get_map_idx(i, 0)] = new_h / new_cnt; + map[get_map_idx(i, 1)] = new_v / new_cnt; + map[get_map_idx(i, 2)] = 1; + } + } + if (valid < 0.5) { + map[get_map_idx(i, 0)] = 0; + map[get_map_idx(i, 1)] = ${initial_variance}; + map[get_map_idx(i, 2)] = 0; + } + """ + ).substitute(max_variance=max_variance, initial_variance=initial_variance), + name="average_map_kernel", + ) + return average_map_kernel + + +def dilation_filter_kernel(width, height, dilation_size): + dilation_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap, raw U newmask", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + newmap[get_map_idx(i, 0)] = h; + if (valid < 0.5) { + U distance = 100; + U near_value = 0; + for (int dy = -${dilation_size}; dy <= ${dilation_size}; dy++) { + for (int dx = -${dilation_size}; dx <= ${dilation_size}; dx++) { + int idx = get_relative_map_idx(i, dx, dy, 0); + if (!is_inside(idx)) {continue;} + U valid = mask[idx]; + if(valid > 0.5 && dx + dy < distance) { + distance = dx + dy; + near_value = map[idx]; + } + } + } + if(distance < 100) { + newmap[get_map_idx(i, 0)] = near_value; + newmask[get_map_idx(i, 0)] = 1.0; + } + } + """ + ).substitute(dilation_size=dilation_size), + name="dilation_filter_kernel", + ) + return dilation_filter_kernel + + +def normal_filter_kernel(width, height, resolution): + normal_filter_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U mask", + out_params="raw U newmap", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_relative_map_idx(int idx, int dx, int dy, int layer_n) { + const int layer = ${width} * ${height}; + const int relative_idx = idx + ${width} * dy + dx; + return layer * layer_n + relative_idx; + } + __device__ bool is_inside(int idx) { + int idx_x = idx / ${width}; + int idx_y = idx % ${width}; + if (idx_x <= 0 || idx_x >= ${width} - 1) { + return false; + } + if (idx_y <= 0 || idx_y >= ${height} - 1) { + return false; + } + return true; + } + __device__ float resolution() { + return ${resolution}; + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + U h = map[get_map_idx(i, 0)]; + U valid = mask[get_map_idx(i, 0)]; + if (valid > 0.5) { + int idx_x = get_relative_map_idx(i, 1, 0, 0); + int idx_y = get_relative_map_idx(i, 0, 1, 0); + if (!is_inside(idx_x) || !is_inside(idx_y)) { return; } + float dzdx = (map[idx_x] - h); + float dzdy = (map[idx_y] - h); + float nx = -dzdy / resolution(); + float ny = -dzdx / resolution(); + float nz = 1; + float norm = sqrt((nx * nx) + (ny * ny) + 1); + newmap[get_map_idx(i, 0)] = nx / norm; + newmap[get_map_idx(i, 1)] = ny / norm; + newmap[get_map_idx(i, 2)] = nz / norm; + } + """ + ).substitute(), + name="normal_filter_kernel", + ) + return normal_filter_kernel + + +def polygon_mask_kernel(width, height, resolution): + polygon_mask_kernel = cp.ElementwiseKernel( + in_params="raw U polygon, raw U center_x, raw U center_y, raw int16 polygon_n, raw U polygon_bbox", + out_params="raw U mask", + preamble=string.Template( + """ + __device__ struct Point + { + int x; + int y; + }; + // Given three colinear points p, q, r, the function checks if + // point q lies on line segment 'pr' + __device__ bool onSegment(Point p, Point q, Point r) + { + if (q.x <= max(p.x, r.x) && q.x >= min(p.x, r.x) && + q.y <= max(p.y, r.y) && q.y >= min(p.y, r.y)) + return true; + return false; + } + // To find orientation of ordered triplet (p, q, r). + // The function returns following values + // 0 --> p, q and r are colinear + // 1 --> Clockwise + // 2 --> Counterclockwise + __device__ int orientation(Point p, Point q, Point r) + { + int val = (q.y - p.y) * (r.x - q.x) - + (q.x - p.x) * (r.y - q.y); + if (val == 0) return 0; // colinear + return (val > 0)? 1: 2; // clock or counterclock wise + } + // The function that returns true if line segment 'p1q1' + // and 'p2q2' intersect. + __device__ bool doIntersect(Point p1, Point q1, Point p2, Point q2) + { + // Find the four orientations needed for general and + // special cases + int o1 = orientation(p1, q1, p2); + int o2 = orientation(p1, q1, q2); + int o3 = orientation(p2, q2, p1); + int o4 = orientation(p2, q2, q1); + // General case + if (o1 != o2 && o3 != o4) + return true; + // Special Cases + // p1, q1 and p2 are colinear and p2 lies on segment p1q1 + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + // p1, q1 and p2 are colinear and q2 lies on segment p1q1 + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + // p2, q2 and p1 are colinear and p1 lies on segment p2q2 + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + // p2, q2 and q1 are colinear and q1 lies on segment p2q2 + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + return false; // Doesn't fall in any of the above cases + } + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + + __device__ int get_idx_x(int idx){ + int idx_x = idx / ${width}; + return idx_x; + } + + __device__ int get_idx_y(int idx){ + int idx_y = idx % ${width}; + return idx_y; + } + + __device__ float16 clamp(float16 x, float16 min_x, float16 max_x) { + + return max(min(x, max_x), min_x); + } + __device__ float16 round(float16 x) { + return (int)x + (int)(2 * (x - (int)x)); + } + __device__ int get_x_idx(float16 x, float16 center) { + const float resolution = ${resolution}; + const float width = ${width}; + int i = (x - center) / resolution + 0.5 * width; + return i; + } + __device__ int get_y_idx(float16 y, float16 center) { + const float resolution = ${resolution}; + const float height = ${height}; + int i = (y - center) / resolution + 0.5 * height; + return i; + } + __device__ int get_idx(float16 x, float16 y, float16 center_x, float16 center_y) { + int idx_x = clamp(get_x_idx(x, center_x), 0, ${width} - 1); + int idx_y = clamp(get_y_idx(y, center_y), 0, ${height} - 1); + return ${width} * idx_x + idx_y; + } + + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + // Point p = {get_idx_x(i, center_x[0]), get_idx_y(i, center_y[0])}; + Point p = {get_idx_x(i), get_idx_y(i)}; + Point extreme = {100000, p.y}; + int bbox_min_idx = get_idx(polygon_bbox[0], polygon_bbox[1], center_x[0], center_y[0]); + int bbox_max_idx = get_idx(polygon_bbox[2], polygon_bbox[3], center_x[0], center_y[0]); + Point bmin = {get_idx_x(bbox_min_idx), get_idx_y(bbox_min_idx)}; + Point bmax = {get_idx_x(bbox_max_idx), get_idx_y(bbox_max_idx)}; + if (p.x < bmin.x || p.x > bmax.x || p.y < bmin.y || p.y > bmax.y){ + mask[i] = 0; + return; + } + else { + int intersect_cnt = 0; + for (int j = 0; j < polygon_n[0]; j++) { + Point p1, p2; + int i1 = get_idx(polygon[j * 2 + 0], polygon[j * 2 + 1], center_x[0], center_y[0]); + p1.x = get_idx_x(i1); + p1.y = get_idx_y(i1); + int j2 = (j + 1) % polygon_n[0]; + int i2 = get_idx(polygon[j2 * 2 + 0], polygon[j2 * 2 + 1], center_x[0], center_y[0]); + p2.x = get_idx_x(i2); + p2.y = get_idx_y(i2); + if (doIntersect(p1, p2, p, extreme)) + { + // If the point 'p' is colinear with line segment 'i-next', + // then check if it lies on segment. If it lies, return true, + // otherwise false + if (orientation(p1, p, p2) == 0) { + if (onSegment(p1, p, p2)){ + mask[i] = 1; + return; + } + } + else if(((p1.y <= p.y) && (p2.y > p.y)) || ((p1.y > p.y) && (p2.y <= p.y))){ + intersect_cnt++; + } + } + } + if (intersect_cnt % 2 == 0) { mask[i] = 0; } + else { mask[i] = 1; } + } + """ + ).substitute(a=1), + name="polygon_mask_kernel", + ) + return polygon_mask_kernel + + + +def image_to_map_correspondence_kernel(resolution, width, height, tolerance_z_collision): + """ + This function calculates the correspondence between the image and the map. + It takes in the resolution, width, height, and tolerance_z_collision as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _image_to_map_correspondence_kernel = cp.ElementwiseKernel( + in_params="raw U map, raw U x1, raw U y1, raw U z1, raw U P, raw U K, raw U D, raw U image_height, raw U image_width, raw U center", + out_params="raw U uv_correspondence, raw B valid_correspondence", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + __device__ bool is_inside_map(int x, int y) { + return (x >= 0 && y >= 0 && x<${width} && y<${height}); + } + __device__ float get_l2_distance(int x0, int y0, int x1, int y1) { + float dx = x0-x1; + float dy = y0-y1; + return sqrt( dx*dx + dy*dy); + } + """ + ).substitute(width=width, height=height, resolution=resolution), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + + // return if gridcell has no valid height + if (map[get_map_idx(i, 2)] != 1){ + return; + } + + // get current cell position + int y0 = i % ${width}; + int x0 = i / ${width}; + + // gridcell 3D point in worldframe TODO reverse x and y + float p1 = (x0-(${width}/2)) * ${resolution} + center[0]; + float p2 = (y0-(${height}/2)) * ${resolution} + center[1]; + float p3 = map[cell_idx] + center[2]; + + // reproject 3D point into image plane + float u = p1 * P[0] + p2 * P[1] + p3 * P[2] + P[3]; + float v = p1 * P[4] + p2 * P[5] + p3 * P[6] + P[7]; + float d = p1 * P[8] + p2 * P[9] + p3 * P[10] + P[11]; + + // filter point behind image plane + if (d <= 0) { + return; + } + u = u/d; + v = v/d; + + // Check if D is all zeros + bool is_D_zero = (D[0] == 0 && D[1] == 0 && D[2] == 0 && D[3] == 0 && D[4] == 0); + + // Apply undistortion using distortion matrix D if not all zeros + if (!is_D_zero) { + float k1 = D[0]; + float k2 = D[1]; + float p1 = D[2]; + float p2 = D[3]; + float k3 = D[4]; + float fx = K[0]; + float fy = K[4]; + float cx = K[2]; + float cy = K[5]; + float x = (u - cx) / fx; + float y = (v - cy) / fy; + float r2 = x * x + y * y; + float radial_distortion = 1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + float u_corrected = x * radial_distortion + 2 * p1 * x * y + p2 * (r2 + 2 * x * x); + float v_corrected = y * radial_distortion + 2 * p2 * x * y + p1 * (r2 + 2 * y * y); + u = fx * u_corrected + cx; + v = fy * v_corrected + cy; + } + + // filter point next to image plane + if ((u < 0) || (v < 0) || (u >= image_width) || (v >= image_height)){ + return; + } + + int y0_c = y0; + int x0_c = x0; + float total_dis = get_l2_distance(x0_c, y0_c, x1,y1); + float z0 = map[cell_idx]; + float delta_z = z1-z0; + + + // bresenham algorithm to iterate over cells in line between camera center and current gridmap cell + // https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm + int dx = abs(x1-x0); + int sx = x0 < x1 ? 1 : -1; + int dy = -abs(y1 - y0); + int sy = y0 < y1 ? 1 : -1; + int error = dx + dy; + + bool is_valid = true; + + // iterate over all cells along line + while (1){ + // assumption we do not need to check the height for camera center cell + if (x0 == x1 && y0 == y1){ + break; + } + + // check if height is invalid + if (is_inside_map(x0,y0)){ + int idx = y0 + (x0 * ${width}); + if (map[get_map_idx(idx, 2)]){ + float dis = get_l2_distance(x0_c, y0_c, x0, y0); + float rayheight = z0 + ( dis / total_dis * delta_z); + if ( map[idx] - ${tolerance_z_collision} > rayheight){ + is_valid = false; + break; + } + } + } + + + // computation of next gridcell index in line + int e2 = 2 * error; + if (e2 >= dy){ + if(x0 == x1){ + break; + } + error = error + dy; + x0 = x0 + sx; + } + if (e2 <= dx){ + if (y0 == y1){ + break; + } + error = error + dx; + y0 = y0 + sy; + } + } + + // mark the correspondence + uv_correspondence[get_map_idx(i, 0)] = u; + uv_correspondence[get_map_idx(i, 1)] = v; + valid_correspondence[get_map_idx(i, 0)] = is_valid; + """ + ).substitute(height=height, width=width, resolution=resolution, tolerance_z_collision=tolerance_z_collision), + name="image_to_map_correspondence_kernel", + ) + return _image_to_map_correspondence_kernel + + +def average_correspondences_to_map_kernel(width, height): + """ + This function calculates the average correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _average_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(), + name="average_correspondences_to_map_kernel", + ) + return _average_correspondences_to_map_kernel + + +def exponential_correspondences_to_map_kernel(width, height, alpha): + """ + This function calculates the exponential correspondences to the map. + It takes in the width, height, and alpha as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _exponential_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_mono, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + int idx = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)] * (1-${alpha}) + ${alpha} * image_mono[idx]; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + + """ + ).substitute(alpha=alpha), + name="exponential_correspondences_to_map_kernel", + ) + return _exponential_correspondences_to_map_kernel + + +def color_correspondences_to_map_kernel(width, height): + """ + This function calculates the color correspondences to the map. + It takes in the width and height as parameters. + The function returns a kernel that can be used to perform the correspondence calculation. + """ + _color_correspondences_to_map_kernel = cp.ElementwiseKernel( + in_params="raw U sem_map, raw U map_idx, raw U image_rgb, raw U uv_correspondence, raw B valid_correspondence, raw U image_height, raw U image_width", + out_params="raw U new_sem_map", + preamble=string.Template( + """ + __device__ int get_map_idx(int idx, int layer_n) { + const int layer = ${width} * ${height}; + return layer * layer_n + idx; + } + """ + ).substitute(width=width, height=height), + operation=string.Template( + """ + int cell_idx = get_map_idx(i, 0); + if (valid_correspondence[cell_idx]){ + int cell_idx_2 = get_map_idx(i, 1); + + int idx_red = int(uv_correspondence[cell_idx]) + int(uv_correspondence[cell_idx_2]) * image_width; + int idx_green = image_width * image_height + idx_red; + int idx_blue = image_width * image_height * 2 + idx_red; + + unsigned int r = image_rgb[idx_red]; + unsigned int g = image_rgb[idx_green]; + unsigned int b = image_rgb[idx_blue]; + + unsigned int rgb = (r<<16) + (g << 8) + b; + float rgb_ = __uint_as_float(rgb); + new_sem_map[get_map_idx(i, map_idx)] = rgb_; + }else{ + new_sem_map[get_map_idx(i, map_idx)] = sem_map[get_map_idx(i, map_idx)]; + } + """ + ).substitute(), + name="color_correspondences_to_map_kernel", + ) + return _color_correspondences_to_map_kernel diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py index 5fc3c110..52fc2664 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py @@ -289,12 +289,12 @@ def update(self): self.true_map_length = self.true_cell_n * self.resolution -if __name__ == "__main__": - param = Parameter() - print(param) - print(param.resolution) - param.set_value("resolution", 0.1) - print(param.resolution) +# if __name__ == "__main__": +# param = Parameter() +# print(param) +# print(param.resolution) +# param.set_value("resolution", 0.1) +# print(param.resolution) - print("names ", param.get_names()) - print("types ", param.get_types()) +# print("names ", param.get_names()) +# print("types ", param.get_types()) diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/traversability_polygon.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/traversability_polygon.py index f39c99cc..be874f1b 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/traversability_polygon.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/traversability_polygon.py @@ -46,7 +46,7 @@ def calculate_area(polygon): def calculate_untraversable_polygon(over_thresh): x, y = cp.where(over_thresh > 0.5) points = cp.stack([x, y]).T - convex_hull = MultiPoint(points).convex_hull + convex_hull = MultiPoint(points.get()).convex_hull if convex_hull.is_empty or convex_hull.geom_type == "Point" or convex_hull.geom_type == "LineString": return None else: diff --git a/elevation_mapping_cupy/setup.py b/elevation_mapping_cupy/setup.py index 8d5a2492..2831eabb 100644 --- a/elevation_mapping_cupy/setup.py +++ b/elevation_mapping_cupy/setup.py @@ -1,8 +1,33 @@ -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup, find_packages -setup_args = generate_distutils_setup( - packages=["elevation_mapping_cupy", "elevation_mapping_cupy.plugins",], package_dir={"": "script"}, +setup( + name="elevation_mapping_cupy", + version="0.0.1", + packages=find_packages(where="script"), + package_dir={"": "script"}, + install_requires=["setuptools"], + zip_safe=True, + maintainer="Your Name", + maintainer_email="your_email@example.com", + description="Description of the elevation_mapping_cupy package.", + license="Apache License 2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + # 'node_name = elevation_mapping_cupy.some_module:main_function', + ], + }, ) -setup(**setup_args) + +# from distutils.core import setup +# from catkin_pkg.python_setup import generate_distutils_setup +# from setuptools import find_packages +# setup_args = generate_distutils_setup( +# packages=["elevation_mapping_cupy", "elevation_mapping_cupy.plugins","elevation_mapping_cupy.kernels","elevation_mapping_cupy.fusion"], package_dir={"": "script"}, + +# ) + +# setup(**setup_args) + + diff --git a/elevation_mapping_cupy/src/elevation_mapping_node.cpp b/elevation_mapping_cupy/src/elevation_mapping_node.cpp index 05fac5d6..ca70b5db 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_node.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_node.cpp @@ -12,16 +12,15 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared("elevation_mapping"); - + py::scoped_interpreter guard{}; // start the interpreter and keep it alive - // elevation_mapping_cupy::ElevationMappingNode mapNode(node); + auto mapNode = std::make_shared(); // Correct instantiation py::gil_scoped_release release; + - // Spin - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - executor.spin(); + // TODO: Create a multi-threaded executor + rclcpp::spin(mapNode); rclcpp::shutdown(); return 0; -} \ No newline at end of file +} + diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 374bf709..e6535d38 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -1,157 +1,187 @@ -// // -// // Copyright (c) 2022, Takahiro Miki. All rights reserved. -// // Licensed under the MIT license. See LICENSE file in the project root for details. -// // - -// #include "elevation_mapping_cupy/elevation_mapping_ros.hpp" - -// // Pybind -// #include - -// // ROS -// #include -// #include -// #include - -// // PCL -// #include - -// #include - -// namespace elevation_mapping_cupy { - -// ElevationMappingNode::ElevationMappingNode(ros::NodeHandle& nh) -// : it_(nh), -// lowpassPosition_(0, 0, 0), -// lowpassOrientation_(0, 0, 0, 1), -// positionError_(0), -// orientationError_(0), -// positionAlpha_(0.1), -// orientationAlpha_(0.1), -// enablePointCloudPublishing_(false), -// isGridmapUpdated_(false) { -// nh_ = nh; - -// std::string pose_topic, map_frame; -// XmlRpc::XmlRpcValue publishers; -// XmlRpc::XmlRpcValue subscribers; -// std::vector map_topics; -// double recordableFps, updateVarianceFps, timeInterval, updatePoseFps, updateGridMapFps, publishStatisticsFps; -// bool enablePointCloudPublishing(false); - -// // Read parameters -// nh.getParam("subscribers", subscribers); -// nh.getParam("publishers", publishers); -// if (!subscribers.valid()) { -// ROS_FATAL("There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); -// } -// if (!publishers.valid()) { -// ROS_FATAL("There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit"); -// } -// nh.param>("initialize_frame_id", initialize_frame_id_, {"base"}); -// nh.param>("initialize_tf_offset", initialize_tf_offset_, {0.0}); -// nh.param("pose_topic", pose_topic, "pose"); -// nh.param("map_frame", mapFrameId_, "map"); -// nh.param("base_frame", baseFrameId_, "base"); -// nh.param("corrected_map_frame", correctedMapFrameId_, "corrected_map"); -// nh.param("initialize_method", initializeMethod_, "cubic"); -// nh.param("position_lowpass_alpha", positionAlpha_, 0.2); -// nh.param("orientation_lowpass_alpha", orientationAlpha_, 0.2); -// nh.param("recordable_fps", recordableFps, 3.0); -// nh.param("update_variance_fps", updateVarianceFps, 1.0); -// nh.param("time_interval", timeInterval, 0.1); -// nh.param("update_pose_fps", updatePoseFps, 10.0); -// nh.param("initialize_tf_grid_size", initializeTfGridSize_, 0.5); -// nh.param("map_acquire_fps", updateGridMapFps, 5.0); -// nh.param("publish_statistics_fps", publishStatisticsFps, 1.0); -// nh.param("enable_pointcloud_publishing", enablePointCloudPublishing, false); -// nh.param("enable_normal_arrow_publishing", enableNormalArrowPublishing_, false); -// nh.param("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_, false); -// nh.param("use_initializer_at_start", useInitializerAtStart_, false); -// nh.param("always_clear_with_initializer", alwaysClearWithInitializer_, false); - -// enablePointCloudPublishing_ = enablePointCloudPublishing; - -// // Iterate all the subscribers -// // here we have to remove all the stuff -// for (auto& subscriber : subscribers) { -// std::string key = subscriber.first; -// auto type = static_cast(subscriber.second["data_type"]); - -// // Initialize subscribers depending on the type -// if (type == "pointcloud") { -// std::string pointcloud_topic = subscriber.second["topic_name"]; -// channels_[key].push_back("x"); -// channels_[key].push_back("y"); -// channels_[key].push_back("z"); -// boost::function f = boost::bind(&ElevationMappingNode::pointcloudCallback, this, _1, key); -// ros::Subscriber sub = nh_.subscribe(pointcloud_topic, 1, f); -// pointcloudSubs_.push_back(sub); -// ROS_INFO_STREAM("Subscribed to PointCloud2 topic: " << pointcloud_topic); -// } -// else if (type == "image") { -// std::string camera_topic = subscriber.second["topic_name"]; -// std::string info_topic = subscriber.second["camera_info_topic_name"]; - -// // Handle compressed images with transport hints -// // We obtain the hint from the last part of the topic name -// std::string transport_hint = "compressed"; -// std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name -// if (ind != std::string::npos) { -// transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part -// camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic -// } else { -// transport_hint = "raw"; // In the default case we assume raw topic -// } - -// // Setup subscriber -// const auto hint = image_transport::TransportHints(transport_hint, ros::TransportHints(), ros::NodeHandle(camera_topic)); -// ImageSubscriberPtr image_sub = std::make_shared(); -// image_sub->subscribe(it_, camera_topic, 1, hint); -// imageSubs_.push_back(image_sub); - -// CameraInfoSubscriberPtr cam_info_sub = std::make_shared(); -// cam_info_sub->subscribe(nh_, info_topic, 1); -// cameraInfoSubs_.push_back(cam_info_sub); - -// std::string channel_info_topic; -// // If there is channel info topic setting, we use it. -// if (subscriber.second.hasMember("channel_info_topic_name")) { -// std::string channel_info_topic = subscriber.second["channel_info_topic_name"]; -// ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(); -// channel_info_sub->subscribe(nh_, channel_info_topic, 1); -// channelInfoSubs_.push_back(channel_info_sub); -// CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); -// sync->registerCallback(boost::bind(&ElevationMappingNode::imageChannelCallback, this, _1, _2, _3)); -// cameraChannelSyncs_.push_back(sync); -// ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ", Channel info topic: " << channel_info_topic); -// } -// else { -// // If there is channels setting, we use it. Otherwise, we use rgb as default. -// if (subscriber.second.hasMember("channels")) { -// const auto& channels = subscriber.second["channels"]; -// for (int32_t i = 0; i < channels.size(); ++i) { -// auto elem = static_cast(channels[i]); -// channels_[key].push_back(elem); -// } -// } -// else { -// channels_[key].push_back("rgb"); -// } -// ROS_INFO_STREAM("Subscribed to Image topic: " << camera_topic << ", Camera info topic: " << info_topic << ". Channel info topic: " << (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")) : channel_info_topic)); -// CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); -// sync->registerCallback(boost::bind(&ElevationMappingNode::imageCallback, this, _1, _2, key)); -// cameraSyncs_.push_back(sync); -// } - - -// } else { -// ROS_WARN_STREAM("Subscriber data_type [" << type << "] Not valid. Supported types: pointcloud, image"); -// continue; -// } -// } - -// map_.initialize(nh_); +// +// Copyright (c) 2022, Takahiro Miki. All rights reserved. +// Licensed under the MIT license. See LICENSE file in the project root for details. +// + + +#include "elevation_mapping_cupy/elevation_mapping_ros.hpp" + +// Pybind +#include + +// ROS2 +#include +#include +#include + +// PCL +#include + +#include + +namespace elevation_mapping_cupy { + +ElevationMappingNode::ElevationMappingNode() + : rclcpp::Node("elevation_mapping_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), + node_(std::shared_ptr(this, [](auto *) {})), + // it_(node_), + lowpassPosition_(0, 0, 0), + lowpassOrientation_(0, 0, 0, 1), + positionError_(0), + orientationError_(0), + positionAlpha_(0.1), + orientationAlpha_(0.1), + enablePointCloudPublishing_(false), + isGridmapUpdated_(false){ + RCLCPP_INFO(this->get_logger(), "Initializing ElevationMappingNode..."); + + tfBroadcaster_ = std::make_shared(*this);// ROS2构造TransformBroadcaster + tfBuffer_ = std::make_shared(this->get_clock()); + tfListener_ = std::make_shared(*tfBuffer_); + + + std::string pose_topic, map_frame; + std::vector map_topics; + double recordableFps, updateVarianceFps, timeInterval, updatePoseFps, updateGridMapFps, publishStatisticsFps; + bool enablePointCloudPublishing(false); + + py::gil_scoped_acquire acquire; + auto math = py::module::import("math"); + double root_two = math.attr("sqrt")(2.0).cast(); + RCLCPP_INFO(this->get_logger(), "The square root of 2 is: %f", root_two); + + this->get_parameter("initialize_frame_id", initialize_frame_id_); + this->get_parameter("initialize_tf_offset", initialize_tf_offset_); + this->get_parameter("pose_topic", pose_topic); + this->get_parameter("map_frame", mapFrameId_); + this->get_parameter("base_frame", baseFrameId_); + this->get_parameter("corrected_map_frame", correctedMapFrameId_); + this->get_parameter("initialize_method", initializeMethod_); + this->get_parameter("position_lowpass_alpha", positionAlpha_); + this->get_parameter("orientation_lowpass_alpha", orientationAlpha_); + this->get_parameter("recordable_fps", recordableFps); + this->get_parameter("update_variance_fps", updateVarianceFps); + this->get_parameter("time_interval", timeInterval); + this->get_parameter("update_pose_fps", updatePoseFps); + this->get_parameter("initialize_tf_grid_size", initializeTfGridSize_); + this->get_parameter("map_acquire_fps", updateGridMapFps); + this->get_parameter("publish_statistics_fps", publishStatisticsFps); + this->get_parameter("enable_pointcloud_publishing", enablePointCloudPublishing); + this->get_parameter("enable_normal_arrow_publishing", enableNormalArrowPublishing_); + this->get_parameter("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_); + this->get_parameter("use_initializer_at_start", useInitializerAtStart_); + this->get_parameter("always_clear_with_initializer", alwaysClearWithInitializer_); + + RCLCPP_INFO(this->get_logger(), "initialize_frame_id: %s", initialize_frame_id_.empty() ? "[]" : initialize_frame_id_[0].c_str()); + RCLCPP_INFO(this->get_logger(), "initialize_tf_offset: [%f, %f, %f, %f]", initialize_tf_offset_[0], initialize_tf_offset_[1], initialize_tf_offset_[2], initialize_tf_offset_[3]); + RCLCPP_INFO(this->get_logger(), "pose_topic: %s", pose_topic.c_str()); + RCLCPP_INFO(this->get_logger(), "map_frame: %s", mapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "base_frame: %s", baseFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "corrected_map_frame: %s", correctedMapFrameId_.c_str()); + RCLCPP_INFO(this->get_logger(), "initialize_method: %s", initializeMethod_.c_str()); + RCLCPP_INFO(this->get_logger(), "position_lowpass_alpha: %f", positionAlpha_); + RCLCPP_INFO(this->get_logger(), "orientation_lowpass_alpha: %f", orientationAlpha_); + RCLCPP_INFO(this->get_logger(), "recordable_fps: %f", recordableFps); + RCLCPP_INFO(this->get_logger(), "update_variance_fps: %f", updateVarianceFps); + RCLCPP_INFO(this->get_logger(), "time_interval: %f", timeInterval); + RCLCPP_INFO(this->get_logger(), "update_pose_fps: %f", updatePoseFps); + RCLCPP_INFO(this->get_logger(), "initialize_tf_grid_size: %f", initializeTfGridSize_); + RCLCPP_INFO(this->get_logger(), "map_acquire_fps: %f", updateGridMapFps); + RCLCPP_INFO(this->get_logger(), "publish_statistics_fps: %f", publishStatisticsFps); + RCLCPP_INFO(this->get_logger(), "enable_pointcloud_publishing: %s", enablePointCloudPublishing ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "enable_normal_arrow_publishing: %s", enableNormalArrowPublishing_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "enable_drift_corrected_TF_publishing: %s", enableDriftCorrectedTFPublishing_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "use_initializer_at_start: %s", useInitializerAtStart_ ? "true" : "false"); + RCLCPP_INFO(this->get_logger(), "always_clear_with_initializer: %s", alwaysClearWithInitializer_ ? "true" : "false"); + + enablePointCloudPublishing_ = enablePointCloudPublishing; + + std::map subscriber_params, publisher_params; + if (!this->get_parameters("subscribers", subscriber_params)) { + RCLCPP_FATAL(this->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); + rclcpp::shutdown(); + } + if (!this->get_parameters("publishers", publisher_params)) { + RCLCPP_FATAL(this->get_logger(), "There aren't any publishers to be configured, the elevation mapping cannot be configured. Exit"); + rclcpp::shutdown(); + } + + for (const auto& name : subscriber_params) { + if (name.first.find("image_topic") != std::string::npos) { + std::string camera_topic = name.second.as_string(); + // setup image subscriber + std::string transport_hint = "compressed"; + std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name + if (ind != std::string::npos) { + transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part + camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic + } else { + transport_hint = "raw"; // In the default case we assume raw topic + } + // Create a standard ROS2 subscription for the image topic + ImageSubscriberPtr image_sub = std::make_shared(this, camera_topic, rmw_qos_profile_sensor_data); + // image_sub->subscribe(this->shared_from_this(), camera_topic, 1); + imageSubs_.push_back(image_sub); + + + + + + std::string info_topic; + std::string channel_info_topic; + // Check if the last character is a digit + if (std::isdigit(name.first.back())) { + char last_char = name.first.back(); + std::string cam_info_name_param = "image_info" + std::string(1, last_char); + std::string channel_info_name_param = "image_channel_info" + std::string(1, last_char); + if (this->get_parameter("subscribers." + cam_info_name_param, info_topic)) { + RCLCPP_INFO(this->get_logger(), "Generated parameter %s: %s", cam_info_name_param.c_str(), info_topic.c_str()); + CameraInfoSubscriberPtr cam_info_sub = std::make_shared(this, info_topic, rmw_qos_profile_sensor_data); + // cam_info_sub->subscribe(this->shared_from_this(), info_topic, 1); + cameraInfoSubs_.push_back(cam_info_sub); + if (this->get_parameter("subscribers." + channel_info_name_param, channel_info_topic)) { + ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(this, channel_info_topic, rmw_qos_profile_sensor_data); + // channel_info_sub->subscribe(this->shared_from_this(), channel_info_topic, 1); + channelInfoSubs_.push_back(channel_info_sub); + CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); + sync->registerCallback(std::bind(&ElevationMappingNode::imageChannelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + cameraChannelSyncs_.push_back(sync); + RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s, Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), channel_info_topic.c_str()); + }else{ + std::string key = name.first; + channels_[key].push_back("rgb"); + // RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s. Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")).c_str() : channel_info_topic.c_str())); + CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); + sync->registerCallback(std::bind(&ElevationMappingNode::imageCallback, this, std::placeholders::_1, std::placeholders::_2, key)); + cameraSyncs_.push_back(sync); + } + }else{ + throw std::runtime_error("Info topic is missing for camera: " + camera_topic); + } + }else{ + throw std::runtime_error("Error: image param numbering at the end "); + } + // generate point cloud subscriber + }else if(name.first.find("pointcloud_topic") != std::string::npos) { + std::string pointcloud_topic = name.second.as_string(); + RCLCPP_INFO(this->get_logger(), "Generated parameter %s:", name.first.c_str()); + std::string key = name.first; + channels_[key].push_back("x"); + channels_[key].push_back("y"); + channels_[key].push_back("z"); + auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + this->pointcloudCallback(msg, key); + }; + auto sub = this->create_subscription(pointcloud_topic, 1, callback); + pointcloudSubs_.push_back(sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to PointCloud2 topic: %s", pointcloud_topic.c_str()); + } + + } + + // get node pointer, pass into Class B + // map_ = std::make_shared(shared_from_this()); + map_->initialize(); // // Register map publishers // for (auto itr = publishers.begin(); itr != publishers.end(); ++itr) { @@ -228,7 +258,10 @@ // } // lastStatisticsPublishedTime_ = ros::Time::now(); // ROS_INFO("[ElevationMappingCupy] finish initialization"); -// } +} + + // namespace elevation_mapping_cupy + // // Setup map publishers // void ElevationMappingNode::setupMapPublishers() { @@ -300,77 +333,80 @@ // mapPubs_[index].publish(msg); // } -// void ElevationMappingNode::pointcloudCallback(const sensor_msgs::PointCloud2& cloud, const std::string& key) { -// // get channels -// auto fields = cloud.fields; -// std::vector channels; +void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key) { + // get channels + auto fields = cloud->fields; + std::vector channels; + + for (size_t it = 0; it < fields.size(); it++) { + auto& field = fields[it]; + channels.push_back(field.name); + } + inputPointCloud(cloud, channels); + + // This is used for publishing as statistics. + pointCloudProcessCounter_++; +} + +void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, + const std::vector& channels) { + auto start = this->now(); + auto* pcl_pc = new pcl::PCLPointCloud2; + pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc); + pcl_conversions::toPCL(*cloud, *pcl_pc); + + // Get channels + auto fields = cloud->fields; + uint array_dim = channels.size(); + + RowMatrixXd points = RowMatrixXd(pcl_pc->width * pcl_pc->height, array_dim); + + for (unsigned int i = 0; i < pcl_pc->width * pcl_pc->height; ++i) { + for (unsigned int j = 0; j < channels.size(); ++j) { + float temp; + uint point_idx = i * pcl_pc->point_step + pcl_pc->fields[j].offset; + memcpy(&temp, &pcl_pc->data[point_idx], sizeof(float)); + points(i, j) = static_cast(temp); + } + } + + // Get pose of sensor in map frame + geometry_msgs::msg::TransformStamped transformStamped; + std::string sensorFrameId = cloud->header.frame_id; + auto timeStamp = cloud->header.stamp; + Eigen::Affine3d transformationSensorToMap; + try { + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, sensorFrameId, tf2::TimePointZero); + transformationSensorToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } + + double positionError{0.0}; + double orientationError{0.0}; + { + std::lock_guard lock(errorMutex_); + positionError = positionError_; + orientationError = orientationError_; + } + + // map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, + // orientationError); + + // if (enableDriftCorrectedTFPublishing_) { + // publishMapToOdom(map_.get_additive_mean_error()); + // } + + RCLCPP_DEBUG(this->get_logger(), "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), + (this->now() - start).seconds()); + RCLCPP_DEBUG(this->get_logger(), "positionError: %f ", positionError); + RCLCPP_DEBUG(this->get_logger(), "orientationError: %f ", orientationError); +} -// for (int it = 0; it < fields.size(); it++) { -// auto& field = fields[it]; -// channels.push_back(field.name); -// } -// inputPointCloud(cloud, channels); -// // This is used for publishing as statistics. -// pointCloudProcessCounter_++; -// } -// void ElevationMappingNode::inputPointCloud(const sensor_msgs::PointCloud2& cloud, -// const std::vector& channels) { -// auto start = ros::Time::now(); -// auto* pcl_pc = new pcl::PCLPointCloud2; -// pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc); -// pcl_conversions::toPCL(cloud, *pcl_pc); - -// // get channels -// auto fields = cloud.fields; -// uint array_dim = channels.size(); - -// RowMatrixXd points = RowMatrixXd(pcl_pc->width * pcl_pc->height, array_dim); - -// for (unsigned int i = 0; i < pcl_pc->width * pcl_pc->height; ++i) { -// for (unsigned int j = 0; j < channels.size(); ++j) { -// float temp; -// uint point_idx = i * pcl_pc->point_step + pcl_pc->fields[j].offset; -// memcpy(&temp, &pcl_pc->data[point_idx], sizeof(float)); -// points(i, j) = static_cast(temp); -// } -// } -// // get pose of sensor in map frame -// tf::StampedTransform transformTf; -// std::string sensorFrameId = cloud.header.frame_id; -// auto timeStamp = cloud.header.stamp; -// Eigen::Affine3d transformationSensorToMap; -// try { -// transformListener_.waitForTransform(mapFrameId_, sensorFrameId, timeStamp, ros::Duration(1.0)); -// transformListener_.lookupTransform(mapFrameId_, sensorFrameId, timeStamp, transformTf); -// poseTFToEigen(transformTf, transformationSensorToMap); -// } catch (tf::TransformException& ex) { -// ROS_ERROR("%s", ex.what()); -// return; -// } - -// double positionError{0.0}; -// double orientationError{0.0}; -// { -// std::lock_guard lock(errorMutex_); -// positionError = positionError_; -// orientationError = orientationError_; -// } -// map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, -// orientationError); - -// if (enableDriftCorrectedTFPublishing_) { -// publishMapToOdom(map_.get_additive_mean_error()); -// } - -// ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), -// (ros::Time::now() - start).toSec()); -// ROS_DEBUG_THROTTLE(1.0, "positionError: %f ", positionError); -// ROS_DEBUG_THROTTLE(1.0, "orientationError: %f ", orientationError); - -// } // void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_msg, // const sensor_msgs::CameraInfoConstPtr& camera_info_msg, @@ -445,24 +481,25 @@ // distortionCoeffs, distortion_model, image.rows, image.cols); // } -// void ElevationMappingNode::imageCallback(const sensor_msgs::ImageConstPtr& image_msg, -// const sensor_msgs::CameraInfoConstPtr& camera_info_msg, -// const std::string& key) { -// auto start = ros::Time::now(); -// inputImage(image_msg, camera_info_msg, channels_[key]); -// ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); -// } - -// void ElevationMappingNode::imageChannelCallback(const sensor_msgs::ImageConstPtr& image_msg, -// const sensor_msgs::CameraInfoConstPtr& camera_info_msg, -// const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg) { -// auto start = ros::Time::now(); -// // Default channels and fusion methods for image is rgb and image_color -// std::vector channels; -// channels = channel_info_msg->channels; -// inputImage(image_msg, camera_info_msg, channels); -// ROS_DEBUG_THROTTLE(1.0, "ElevationMap processed an image in %f sec.", (ros::Time::now() - start).toSec()); -// } +void ElevationMappingNode::imageCallback(const std::shared_ptr& image_msg, + const std::shared_ptr& camera_info_msg, + const std::string& key) { + auto start = this->now(); + // inputImage(image_msg, camera_info_msg, channels_[key]); + RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageCallback processed an image in %f sec.", (this->now() - start).seconds()); +} + + +void ElevationMappingNode::imageChannelCallback(const std::shared_ptr& image_msg, + const std::shared_ptr& camera_info_msg, + const std::shared_ptr& channel_info_msg) { +auto start = this->now(); +// Default channels and fusion methods for image is rgb and image_color +// std::vector channels; +// channels = channel_info_msg->channels; +// inputImage(image_msg, camera_info_msg, channels); +RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); +} // void ElevationMappingNode::updatePose(const ros::TimerEvent&) { // tf::StampedTransform transformTf; @@ -817,4 +854,4 @@ // tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_)); // } -// } // namespace elevation_mapping_cupy +} // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index 16c57332..f0796abb 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -20,9 +20,10 @@ namespace elevation_mapping_cupy { -ElevationMappingWrapper::ElevationMappingWrapper() {} +ElevationMappingWrapper::ElevationMappingWrapper(rclcpp::Node::SharedPtr node) +:node_(node) {} -void ElevationMappingWrapper::initialize(rclcpp::Node::SharedPtr node) { +void ElevationMappingWrapper::initialize() { // Add the elevation_mapping_cupy path to sys.path auto threading = py::module::import("threading"); py::gil_scoped_acquire acquire; @@ -34,148 +35,154 @@ void ElevationMappingWrapper::initialize(rclcpp::Node::SharedPtr node) { path.attr("insert")(0, module_path); auto elevation_mapping = py::module::import("elevation_mapping_cupy.elevation_mapping"); - auto parameter = py::module::import("elevation_mapping_cupy.parameter"); - param_ = parameter.attr("Parameter")(); - setParameters(node); - map_ = elevation_mapping.attr("ElevationMap")(param_); + // auto parameter = py::module::import("math"); + + // auto param_2 = parameter.attr("Parameter")(); + // setParameters(param_v2); + auto params = setParameters(); + + // map_ = elevation_mapping.attr("ElevationMap")(param_v2); } // /** // * Load ros parameters into Parameter class. // * Search for the same name within the name space. // */ -void ElevationMappingWrapper::setParameters(rclcpp::Node::SharedPtr node) { +py::object ElevationMappingWrapper::setParameters() { + auto parameter = py::module::import("elevation_mapping_cupy.parameter"); + auto param_2 = parameter.attr("Parameter")(); + return param_2; // Get all parameters names and types. - py::list paramNames = param_.attr("get_names")(); - py::list paramTypes = param_.attr("get_types")(); - py::gil_scoped_acquire acquire; - - // Try to find the parameter in the ROS parameter server. - // If there was a parameter, set it to the Parameter variable. - for (size_t i = 0; i < paramNames.size(); i++) { - std::string type = py::cast(paramTypes[i]); - std::string name = py::cast(paramNames[i]); - if (type == "float") { - float param; - if (node->get_parameter(name, param)) { - param_.attr("set_value")(name, param); - } - } else if (type == "str") { - std::string param; - if (node->get_parameter(name, param)) { - param_.attr("set_value")(name, param); - } - } else if (type == "bool") { - bool param; - if (node->get_parameter(name, param)) { - param_.attr("set_value")(name, param); - } - } else if (type == "int") { - int param; - if (node->get_parameter(name, param)) { - param_.attr("set_value")(name, param); - } - } - } + // py::list paramNames = param_.attr("get_names")(); + // py::list paramTypes = param_.attr("get_types")(); + // py::gil_scoped_acquire acquire; + + // // Try to find the parameter in the ROS parameter server. + // // If there was a parameter, set it to the Parameter variable. + // for (size_t i = 0; i < paramNames.size(); i++) { + // std::string type = py::cast(paramTypes[i]); + // std::string name = py::cast(paramNames[i]); + // if (type == "float") { + // float param; + // if (node_->get_parameter(name, param)) { + // param_.attr("set_value")(name, param); + // } + // } else if (type == "str") { + // std::string param; + // if (node_->get_parameter(name, param)) { + // param_.attr("set_value")(name, param); + // } + // } else if (type == "bool") { + // bool param; + // if (node_->get_parameter(name, param)) { + // param_.attr("set_value")(name, param); + // } + // } else if (type == "int") { + // int param; + // if (node_->get_parameter(name, param)) { + // param_.attr("set_value")(name, param); + // } + // } + // } - rclcpp::Parameter subscribers; - node->get_parameter("subscribers", subscribers); - - py::dict sub_dict; - auto subscribers_array = subscribers.as_string_array(); - for (const auto& subscriber : subscribers_array) { - const char* const name = subscriber.c_str(); - if (!sub_dict.contains(name)) { - sub_dict[name] = py::dict(); - } - std::map subscriber_params; - node->get_parameters(name, subscriber_params); - for (const auto& param_pair : subscriber_params) { - const char* const param_name = param_pair.first.c_str(); - const auto& param_value = param_pair.second; - std::vector arr; - switch (param_value.get_type()) { - case rclcpp::ParameterType::PARAMETER_STRING: - sub_dict[name][param_name] = param_value.as_string(); - break; - case rclcpp::ParameterType::PARAMETER_INTEGER: - sub_dict[name][param_name] = param_value.as_int(); - break; - case rclcpp::ParameterType::PARAMETER_DOUBLE: - sub_dict[name][param_name] = param_value.as_double(); - break; - case rclcpp::ParameterType::PARAMETER_BOOL: - sub_dict[name][param_name] = param_value.as_bool(); - break; - case rclcpp::ParameterType::PARAMETER_STRING_ARRAY: - for (const auto& elem : param_value.as_string_array()) { - arr.push_back(elem); - } - sub_dict[name][param_name] = arr; - arr.clear(); - break; - default: - sub_dict[name][param_name] = py::cast(param_value); - break; - } - } - } - param_.attr("subscriber_cfg") = sub_dict; - - - // point cloud channel fusion - if (!node->has_parameter("pointcloud_channel_fusions")) { - RCLCPP_WARN(node->get_logger(), "No pointcloud_channel_fusions parameter found. Using default values."); - } else { - rclcpp::Parameter pointcloud_channel_fusion; - node->get_parameter("pointcloud_channel_fusions", pointcloud_channel_fusion); - - py::dict pointcloud_channel_fusion_dict; - auto pointcloud_channel_fusion_map = pointcloud_channel_fusion.as_string_array(); - for (const auto& channel_fusion : pointcloud_channel_fusion_map) { - const char* const fusion_name = channel_fusion.c_str(); - std::string fusion; - node->get_parameter(fusion_name, fusion); - if (!pointcloud_channel_fusion_dict.contains(fusion_name)) { - pointcloud_channel_fusion_dict[fusion_name] = fusion; - } - } - RCLCPP_INFO_STREAM(node->get_logger(), "pointcloud_channel_fusion_dict: " << pointcloud_channel_fusion_dict); - param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict; - } + // rclcpp::Parameter subscribers; + // node_->get_parameter("subscribers", subscribers); + + // py::dict sub_dict; + // auto subscribers_array = subscribers.as_string_array(); + // for (const auto& subscriber : subscribers_array) { + // const char* const name = subscriber.c_str(); + // if (!sub_dict.contains(name)) { + // sub_dict[name] = py::dict(); + // } + // std::map subscriber_params; + // node_->get_parameters(name, subscriber_params); + // for (const auto& param_pair : subscriber_params) { + // const char* const param_name = param_pair.first.c_str(); + // const auto& param_value = param_pair.second; + // std::vector arr; + // switch (param_value.get_type()) { + // case rclcpp::ParameterType::PARAMETER_STRING: + // sub_dict[name][param_name] = param_value.as_string(); + // break; + // case rclcpp::ParameterType::PARAMETER_INTEGER: + // sub_dict[name][param_name] = param_value.as_int(); + // break; + // case rclcpp::ParameterType::PARAMETER_DOUBLE: + // sub_dict[name][param_name] = param_value.as_double(); + // break; + // case rclcpp::ParameterType::PARAMETER_BOOL: + // sub_dict[name][param_name] = param_value.as_bool(); + // break; + // case rclcpp::ParameterType::PARAMETER_STRING_ARRAY: + // for (const auto& elem : param_value.as_string_array()) { + // arr.push_back(elem); + // } + // sub_dict[name][param_name] = arr; + // arr.clear(); + // break; + // default: + // sub_dict[name][param_name] = py::cast(param_value); + // break; + // } + // } + // } + // param_.attr("subscriber_cfg") = sub_dict; + + + // // point cloud channel fusion + // if (!node_->has_parameter("pointcloud_channel_fusions")) { + // RCLCPP_WARN(node_->get_logger(), "No pointcloud_channel_fusions parameter found. Using default values."); + // } else { + // rclcpp::Parameter pointcloud_channel_fusion; + // node_->get_parameter("pointcloud_channel_fusions", pointcloud_channel_fusion); + + // py::dict pointcloud_channel_fusion_dict; + // auto pointcloud_channel_fusion_map = pointcloud_channel_fusion.as_string_array(); + // for (const auto& channel_fusion : pointcloud_channel_fusion_map) { + // const char* const fusion_name = channel_fusion.c_str(); + // std::string fusion; + // node_->get_parameter(fusion_name, fusion); + // if (!pointcloud_channel_fusion_dict.contains(fusion_name)) { + // pointcloud_channel_fusion_dict[fusion_name] = fusion; + // } + // } + // RCLCPP_INFO_STREAM(node_->get_logger(), "pointcloud_channel_fusion_dict: " << pointcloud_channel_fusion_dict); + // param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict; + // } - // image channel fusion - if (!node->has_parameter("image_channel_fusions")) { - RCLCPP_WARN(node->get_logger(), "No image_channel_fusions parameter found. Using default values."); - } else { - rclcpp::Parameter image_channel_fusion; - node->get_parameter("image_channel_fusions", image_channel_fusion); - - py::dict image_channel_fusion_dict; - auto image_channel_fusion_map = image_channel_fusion.as_string_array(); - for (const auto& channel_fusion : image_channel_fusion_map) { - const char* const channel_fusion_name = channel_fusion.c_str(); - std::string fusion; - node->get_parameter(channel_fusion_name, fusion); - if (!image_channel_fusion_dict.contains(channel_fusion_name)) { - image_channel_fusion_dict[channel_fusion_name] = fusion; - } - } - RCLCPP_INFO_STREAM(node->get_logger(), "image_channel_fusion_dict: " << image_channel_fusion_dict); - param_.attr("image_channel_fusions") = image_channel_fusion_dict; - } - - param_.attr("update")(); - resolution_ = py::cast(param_.attr("get_value")("resolution")); - map_length_ = py::cast(param_.attr("get_value")("true_map_length")); - map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); - - node->declare_parameter("enable_normal", false); - node->declare_parameter("enable_normal_color", false); - enable_normal_ = node->get_parameter("enable_normal").as_bool(); - enable_normal_color_ = node->get_parameter("enable_normal_color").as_bool(); + // // image channel fusion + // if (!node_->has_parameter("image_channel_fusions")) { + // RCLCPP_WARN(node_->get_logger(), "No image_channel_fusions parameter found. Using default values."); + // } else { + // rclcpp::Parameter image_channel_fusion; + // node_->get_parameter("image_channel_fusions", image_channel_fusion); + + // py::dict image_channel_fusion_dict; + // auto image_channel_fusion_map = image_channel_fusion.as_string_array(); + // for (const auto& channel_fusion : image_channel_fusion_map) { + // const char* const channel_fusion_name = channel_fusion.c_str(); + // std::string fusion; + // node_->get_parameter(channel_fusion_name, fusion); + // if (!image_channel_fusion_dict.contains(channel_fusion_name)) { + // image_channel_fusion_dict[channel_fusion_name] = fusion; + // } + // } + // RCLCPP_INFO_STREAM(node_->get_logger(), "image_channel_fusion_dict: " << image_channel_fusion_dict); + // param_.attr("image_channel_fusions") = image_channel_fusion_dict; + // } + + // param_.attr("update")(); + // resolution_ = py::cast(param_.attr("get_value")("resolution")); + // map_length_ = py::cast(param_.attr("get_value")("true_map_length")); + // map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); + + // node_->declare_parameter("enable_normal", false); + // node_->declare_parameter("enable_normal_color", false); + // enable_normal_ = node_->get_parameter("enable_normal").as_bool(); + // enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool(); } From 59d9a21fe681fd9abaff3180738339ce39186088 Mon Sep 17 00:00:00 2001 From: amilearning Date: Tue, 5 Nov 2024 23:49:41 +0100 Subject: [PATCH 03/14] sub and pub --- .../elevation_mapping.py | 31 +- .../lib/elevation_mapping_cupy/parameter.py | 26 +- .../config/core/core_param.yaml | 23 +- .../elevation_mapping_ros.hpp | 4 +- .../elevation_mapping_wrapper.hpp | 9 +- .../elevation_mapping.py | 29 +- .../elevation_mapping_cupy/parameter.py | 10 +- .../src/elevation_mapping_ros.cpp | 342 +++++++++--------- .../src/elevation_mapping_wrapper.cpp | 199 +++++----- 9 files changed, 366 insertions(+), 307 deletions(-) diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py index 4d704739..06f81dfb 100644 --- a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/elevation_mapping.py @@ -85,7 +85,7 @@ def __init__(self, param: Parameter): self.elevation_map[1] += self.initial_variance self.elevation_map[3] += 1.0 - # overlap clearance + # # overlap clearance cell_range = int(self.param.overlap_clear_range_xy / self.resolution) cell_range = np.clip(cell_range, 0, self.cell_n) self.cell_min = self.cell_n // 2 - cell_range // 2 @@ -95,27 +95,28 @@ def __init__(self, param: Parameter): self.mean_error = 0.0 self.additive_mean_error = 0.0 + self.compile_kernels() - self.compile_image_kernels() + # self.compile_image_kernels() - self.semantic_map.initialize_fusion() + # self.semantic_map.initialize_fusion() - weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') - param.load_weights(weight_file) + # weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') + # param.load_weights(weight_file) - if param.use_chainer: - self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) - else: - self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) - self.untraversable_polygon = xp.zeros((1, 2)) + # if param.use_chainer: + # self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) + # else: + # self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) + # self.untraversable_polygon = xp.zeros((1, 2)) - # Plugins - self.plugin_manager = PluginManager(cell_n=self.cell_n) - plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') - self.plugin_manager.load_plugin_settings(plugin_config_file) + # # Plugins + # self.plugin_manager = PluginManager(cell_n=self.cell_n) + # plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') + # self.plugin_manager.load_plugin_settings(plugin_config_file) - self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") + # self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") def clear(self): """Reset all the layers of the elevation & the semantic map.""" diff --git a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py index 5fc3c110..2193ad02 100644 --- a/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py +++ b/elevation_mapping_cupy/build/lib/elevation_mapping_cupy/parameter.py @@ -171,13 +171,13 @@ class Parameter(Serializable): time_interval: float = 0.1 # Time layer is updated with this interval. max_variance: float = 1.0 # maximum variance for each cell. - dilation_size: float = 2 # dilation filter size before traversability filter. - dilation_size_initialize: float = 10 # dilation size after the init. + dilation_size: int = 2 # dilation filter size before traversability filter. + dilation_size_initialize: int = 10 # dilation size after the init. drift_compensation_alpha: float = 1.0 # drift compensation alpha for smoother update of drift compensation. traversability_inlier: float = 0.1 # cells with higher traversability are used for drift compensation. - wall_num_thresh: float = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. - min_height_drift_cnt: float = 100 # drift compensation only happens if the valid cells are more than this number. + wall_num_thresh: int = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. + min_height_drift_cnt: int = 100 # drift compensation only happens if the valid cells are more than this number. max_ray_length: float = 2.0 # maximum length for ray tracing. cleanup_step: float = 0.01 # substitute this value from validity layer at visibility cleanup. @@ -210,7 +210,7 @@ class Parameter(Serializable): position_noise_thresh: float = 0.1 # if the position change is bigger than this value, the drift compensation happens. orientation_noise_thresh: float = 0.1 # if the orientation change is bigger than this value, the drift compensation happens. - plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin + # plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin weight_file: str = "config/weights.dat" # weight file for traversability filter initial_variance: float = 10.0 # initial variance for each cell. @@ -289,12 +289,12 @@ def update(self): self.true_map_length = self.true_cell_n * self.resolution -if __name__ == "__main__": - param = Parameter() - print(param) - print(param.resolution) - param.set_value("resolution", 0.1) - print(param.resolution) +# if __name__ == "__main__": +# param = Parameter() +# print(param) +# print(param.resolution) +# param.set_value("resolution", 0.1) +# print(param.resolution) - print("names ", param.get_names()) - print("types ", param.get_types()) +# print("names ", param.get_names()) +# print("types ", param.get_types()) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index c9228427..4c0d8302 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,6 +1,14 @@ elevation_mapping: ros__parameters: #### Basic parameters ######## + cell_n: 10000 # number of cells in the map. + data_type: 'float32' # data type for the map. + average_weight: 0.5 + drift_compensation_variance_inlier: 0.1 + checker_layer: 'traversability' # layer name for checking the validity of the cell. + min_filter_size: 5 # size of the filter for min filter. + min_filter_iteration: 3 # number of iterations for min filter. + initialized_variance: 10.0 # initial variance for each cell. resolution: 0.04 # resolution in m. map_length: 8.0 # map's size in m. sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). @@ -95,11 +103,14 @@ elevation_mapping: # channel_info_topic_name: '/camera/channel_info' subscribers: - pointcloud_topic1: '/a200/sensors/camera_1/points' - - image_topic1: '/a200/sensors/camera_0/color/image' - image_info1: '/a200/sensors/camera_0/color/camera_info' - # image_channel_info1: '/front_cam/channel_info' + pointcloud1: + topic_name: '/a200/sensors/camera_1/points' + data_type: 'pointcloud' + image1: + topic_name: '/a200/sensors/camera_0/color/image' + info_name: '/a200/sensors/camera_0/color/camera_info' + data_type: 'image' + # channel_name: '/front_cam/channel_info' # image_topic2: '/camera/rgb/image_raw2' # image_info2: '/camera/depth/camera_info2' @@ -120,7 +131,7 @@ elevation_mapping: # fps: # Publish rate. Use smaller value than `map_acquire_fps`. publishers: - elevation_map_raw: + elevation_map_raw: layers: ['elevation', 'traversability', 'variance','rgb'] basic_layers: ['elevation'] fps: 5.0 diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index 18fd4eaf..b4abac57 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -96,7 +96,7 @@ class ElevationMappingNode : public rclcpp::Node { private: void readParameters(); -// void setupMapPublishers(); + void setupMapPublishers(); void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key); void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector& channels); // void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector& channels); @@ -122,7 +122,7 @@ void imageChannelCallback(const std::shared_ptr& // void initializeWithTF(); // void publishMapToOdom(double error); // void publishStatistics(const ros::TimerEvent&); -// void publishMapOfIndex(int index); + void publishMapOfIndex(int index); // visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index 0623d0f6..a6514843 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -43,9 +43,9 @@ class ElevationMappingWrapper { using RowMatrixXf = Eigen::Matrix; using ColMatrixXf = Eigen::Matrix; - ElevationMappingWrapper(rclcpp::Node::SharedPtr node); + ElevationMappingWrapper(); - void initialize(); + void initialize(const std::shared_ptr& node); void input(const RowMatrixXd& points, const std::vector& channels, const RowMatrixXd& R, const Eigen::VectorXd& t, const double positionNoise, const double orientationNoise); @@ -65,8 +65,9 @@ class ElevationMappingWrapper { void addNormalColorLayer(grid_map::GridMap& map); private: - rclcpp::Node::SharedPtr node_; - py::object setParameters(); + + std::shared_ptr node_; + void setParameters(); py::object map_; py::object param_; diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py index 4d704739..b0b0b40f 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -85,7 +85,7 @@ def __init__(self, param: Parameter): self.elevation_map[1] += self.initial_variance self.elevation_map[3] += 1.0 - # overlap clearance + # # overlap clearance cell_range = int(self.param.overlap_clear_range_xy / self.resolution) cell_range = np.clip(cell_range, 0, self.cell_n) self.cell_min = self.cell_n // 2 - cell_range // 2 @@ -95,27 +95,28 @@ def __init__(self, param: Parameter): self.mean_error = 0.0 self.additive_mean_error = 0.0 + self.compile_kernels() self.compile_image_kernels() - self.semantic_map.initialize_fusion() + # self.semantic_map.initialize_fusion() - weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') - param.load_weights(weight_file) + # weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') + # param.load_weights(weight_file) - if param.use_chainer: - self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) - else: - self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) - self.untraversable_polygon = xp.zeros((1, 2)) + # if param.use_chainer: + # self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) + # else: + # self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) + # self.untraversable_polygon = xp.zeros((1, 2)) - # Plugins - self.plugin_manager = PluginManager(cell_n=self.cell_n) - plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') - self.plugin_manager.load_plugin_settings(plugin_config_file) + # # Plugins + # self.plugin_manager = PluginManager(cell_n=self.cell_n) + # plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') + # self.plugin_manager.load_plugin_settings(plugin_config_file) - self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") + # self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") def clear(self): """Reset all the layers of the elevation & the semantic map.""" diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py index 52fc2664..2193ad02 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py @@ -171,13 +171,13 @@ class Parameter(Serializable): time_interval: float = 0.1 # Time layer is updated with this interval. max_variance: float = 1.0 # maximum variance for each cell. - dilation_size: float = 2 # dilation filter size before traversability filter. - dilation_size_initialize: float = 10 # dilation size after the init. + dilation_size: int = 2 # dilation filter size before traversability filter. + dilation_size_initialize: int = 10 # dilation size after the init. drift_compensation_alpha: float = 1.0 # drift compensation alpha for smoother update of drift compensation. traversability_inlier: float = 0.1 # cells with higher traversability are used for drift compensation. - wall_num_thresh: float = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. - min_height_drift_cnt: float = 100 # drift compensation only happens if the valid cells are more than this number. + wall_num_thresh: int = 100 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. + min_height_drift_cnt: int = 100 # drift compensation only happens if the valid cells are more than this number. max_ray_length: float = 2.0 # maximum length for ray tracing. cleanup_step: float = 0.01 # substitute this value from validity layer at visibility cleanup. @@ -210,7 +210,7 @@ class Parameter(Serializable): position_noise_thresh: float = 0.1 # if the position change is bigger than this value, the drift compensation happens. orientation_noise_thresh: float = 0.1 # if the orientation change is bigger than this value, the drift compensation happens. - plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin + # plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin weight_file: str = "config/weights.dat" # weight file for traversability filter initial_variance: float = 10.0 # initial variance for each cell. diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index e6535d38..760a6dc5 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -21,6 +21,19 @@ namespace elevation_mapping_cupy { +std::vector extract_unique_names(const std::map& subscriber_params) { + std::set unique_names_set; + for (const auto& param : subscriber_params) { + std::size_t pos = param.first.find('.'); + if (pos != std::string::npos) { + std::string name = param.first.substr(0, pos); + unique_names_set.insert(name); + } + } + return std::vector(unique_names_set.begin(), unique_names_set.end()); +} + + ElevationMappingNode::ElevationMappingNode() : rclcpp::Node("elevation_mapping_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), node_(std::shared_ptr(this, [](auto *) {})), @@ -95,7 +108,12 @@ ElevationMappingNode::ElevationMappingNode() RCLCPP_INFO(this->get_logger(), "always_clear_with_initializer: %s", alwaysClearWithInitializer_ ? "true" : "false"); enablePointCloudPublishing_ = enablePointCloudPublishing; - + + // map_ = std::make_shared(); + // map_->initialize(node_); + + + std::map subscriber_params, publisher_params; if (!this->get_parameters("subscribers", subscriber_params)) { RCLCPP_FATAL(this->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); @@ -106,121 +124,117 @@ ElevationMappingNode::ElevationMappingNode() rclcpp::shutdown(); } - for (const auto& name : subscriber_params) { - if (name.first.find("image_topic") != std::string::npos) { - std::string camera_topic = name.second.as_string(); - // setup image subscriber - std::string transport_hint = "compressed"; - std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name - if (ind != std::string::npos) { - transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part - camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic - } else { - transport_hint = "raw"; // In the default case we assume raw topic - } - // Create a standard ROS2 subscription for the image topic - ImageSubscriberPtr image_sub = std::make_shared(this, camera_topic, rmw_qos_profile_sensor_data); - // image_sub->subscribe(this->shared_from_this(), camera_topic, 1); + + auto unique_sub_names = extract_unique_names(subscriber_params); + for (const auto& sub_name : unique_sub_names) { + std::string data_type; + if(this->get_parameter("subscribers." + sub_name + ".data_type", data_type)){ + // image + if(data_type == "image"){ + std::string camera_topic; + std::string info_topic; + this->get_parameter("subscribers." + sub_name + ".topic_name", camera_topic); + this->get_parameter("subscribers." + sub_name + ".info_name", info_topic); + RCLCPP_INFO(this->get_logger(), "camera_topic %s: %s", sub_name.c_str(), camera_topic.c_str()); + RCLCPP_INFO(this->get_logger(), "info_name %s: %s", sub_name.c_str(), info_topic.c_str()); + + // std::string transport_hint = "compressed"; + // std::size_t ind = camera_topic.find(transport_hint); // Find if compressed is in the topic name + // if (ind != std::string::npos) { + // transport_hint = camera_topic.substr(ind, camera_topic.length()); // Get the hint as the last part + // camera_topic.erase(ind - 1, camera_topic.length()); // We remove the hint from the topic + // } else { + // transport_hint = "raw"; // In the default case we assume raw topic + // } + + ImageSubscriberPtr image_sub = std::make_shared(this, camera_topic, rmw_qos_profile_sensor_data); imageSubs_.push_back(image_sub); - - - + CameraInfoSubscriberPtr cam_info_sub = std::make_shared(this, info_topic, rmw_qos_profile_sensor_data); + cameraInfoSubs_.push_back(cam_info_sub); - std::string info_topic; std::string channel_info_topic; - // Check if the last character is a digit - if (std::isdigit(name.first.back())) { - char last_char = name.first.back(); - std::string cam_info_name_param = "image_info" + std::string(1, last_char); - std::string channel_info_name_param = "image_channel_info" + std::string(1, last_char); - if (this->get_parameter("subscribers." + cam_info_name_param, info_topic)) { - RCLCPP_INFO(this->get_logger(), "Generated parameter %s: %s", cam_info_name_param.c_str(), info_topic.c_str()); - CameraInfoSubscriberPtr cam_info_sub = std::make_shared(this, info_topic, rmw_qos_profile_sensor_data); - // cam_info_sub->subscribe(this->shared_from_this(), info_topic, 1); - cameraInfoSubs_.push_back(cam_info_sub); - if (this->get_parameter("subscribers." + channel_info_name_param, channel_info_topic)) { - ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(this, channel_info_topic, rmw_qos_profile_sensor_data); - // channel_info_sub->subscribe(this->shared_from_this(), channel_info_topic, 1); - channelInfoSubs_.push_back(channel_info_sub); - CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); - sync->registerCallback(std::bind(&ElevationMappingNode::imageChannelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cameraChannelSyncs_.push_back(sync); - RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s, Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), channel_info_topic.c_str()); - }else{ - std::string key = name.first; - channels_[key].push_back("rgb"); - // RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s. Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")).c_str() : channel_info_topic.c_str())); - CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); - sync->registerCallback(std::bind(&ElevationMappingNode::imageCallback, this, std::placeholders::_1, std::placeholders::_2, key)); - cameraSyncs_.push_back(sync); - } - }else{ - throw std::runtime_error("Info topic is missing for camera: " + camera_topic); - } + if (this->get_parameter("subscribers." + sub_name + ".channel_name", channel_info_topic)) { + ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(this, channel_info_topic, rmw_qos_profile_sensor_data); + channelInfoSubs_.push_back(channel_info_sub); + CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); + sync->registerCallback(std::bind(&ElevationMappingNode::imageChannelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + cameraChannelSyncs_.push_back(sync); + RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s, Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), channel_info_topic.c_str()); }else{ - throw std::runtime_error("Error: image param numbering at the end "); + std::string key = sub_name; + channels_[key].push_back("rgb"); + // RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s. Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")).c_str() : channel_info_topic.c_str())); + CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); + sync->registerCallback(std::bind(&ElevationMappingNode::imageCallback, this, std::placeholders::_1, std::placeholders::_2, key)); + cameraSyncs_.push_back(sync); } - // generate point cloud subscriber - }else if(name.first.find("pointcloud_topic") != std::string::npos) { - std::string pointcloud_topic = name.second.as_string(); - RCLCPP_INFO(this->get_logger(), "Generated parameter %s:", name.first.c_str()); - std::string key = name.first; - channels_[key].push_back("x"); - channels_[key].push_back("y"); - channels_[key].push_back("z"); - auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { + }else if(data_type == "pointcloud"){ + std::string pointcloud_topic; + this->get_parameter("subscribers." + sub_name + ".topic_name", pointcloud_topic); + std::string key = sub_name; + channels_[key].push_back("x"); + channels_[key].push_back("y"); + channels_[key].push_back("z"); + auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { this->pointcloudCallback(msg, key); }; auto sub = this->create_subscription(pointcloud_topic, 1, callback); pointcloudSubs_.push_back(sub); RCLCPP_INFO(this->get_logger(), "Subscribed to PointCloud2 topic: %s", pointcloud_topic.c_str()); - } + } + } + } + + + + auto unique_pub_names = extract_unique_names(publisher_params); + + // for (const auto& pub_name : unique_pub_names) { + // std::string topic_name = pub_name; + // std::vector layers_list; + // std::vector basic_layers_list; + // double fps; + // this->get_parameter("publishers." + pub_name + ".layers", layers_list) + // this->get_parameter("publishers." + pub_name + ".basic_layers", basic_layers_list) + // this->get_parameter("publishers." + pub_name + ".fps", fps) + // } + for (const auto& pub_name : unique_pub_names) { + std::string topic_name = pub_name; + double fps; + std::vector layers_list; + std::vector basic_layers_list; + + this->get_parameter("publishers." + pub_name + ".layers", layers_list); + this->get_parameter("publishers." + pub_name + ".basic_layers", basic_layers_list); + this->get_parameter("publishers." + pub_name + ".fps", fps); + + if (fps > updateGridMapFps) { + RCLCPP_WARN( + this->get_logger(), + "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " + "fps.", + topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); } - // get node pointer, pass into Class B - // map_ = std::make_shared(shared_from_this()); - map_->initialize(); - -// // Register map publishers -// for (auto itr = publishers.begin(); itr != publishers.end(); ++itr) { -// // Parse params -// std::string topic_name = itr->first; -// std::vector layers_list; -// std::vector basic_layers_list; -// auto layers = itr->second["layers"]; -// auto basic_layers = itr->second["basic_layers"]; -// double fps = itr->second["fps"]; - -// if (fps > updateGridMapFps) { -// ROS_WARN( -// "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " -// "fps.", -// topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); -// } + // Make publishers + auto pub = this->create_publisher(topic_name, 1); + RCLCPP_INFO(this->get_logger(), "Publishing map to topic %s", topic_name.c_str()); + mapPubs_.push_back(pub); -// for (int32_t i = 0; i < layers.size(); ++i) { -// layers_list.push_back(static_cast(layers[i])); -// } + // Register map layers + map_layers_.push_back(layers_list); + map_basic_layers_.push_back(basic_layers_list); -// for (int32_t i = 0; i < basic_layers.size(); ++i) { -// basic_layers_list.push_back(static_cast(basic_layers[i])); -// } + // Register map fps + map_fps_.push_back(fps); + map_fps_unique_.insert(fps); -// // Make publishers -// ros::Publisher pub = nh_.advertise(topic_name, 1); -// mapPubs_.push_back(pub); + } -// // Register map layers -// map_layers_.push_back(layers_list); -// map_basic_layers_.push_back(basic_layers_list); -// // Register map fps -// map_fps_.push_back(fps); -// map_fps_unique_.insert(fps); -// } -// setupMapPublishers(); + setupMapPublishers(); // pointPub_ = nh_.advertise("elevation_map_points", 1); // alivePub_ = nh_.advertise("alive", 1); @@ -264,74 +278,75 @@ ElevationMappingNode::ElevationMappingNode() // // Setup map publishers -// void ElevationMappingNode::setupMapPublishers() { -// // Find the layers with highest fps. -// float max_fps = -1; -// // Create timers for each unique map frequencies -// for (auto fps : map_fps_unique_) { -// // Which publisher to call in the timer callback -// std::vector indices; -// // If this fps is max, update the map layers. -// if (fps >= max_fps) { -// max_fps = fps; -// map_layers_all_.clear(); -// } -// for (int i = 0; i < map_fps_.size(); i++) { -// if (map_fps_[i] == fps) { -// indices.push_back(i); -// // If this fps is max, add layers -// if (fps >= max_fps) { -// for (const auto layer : map_layers_[i]) { -// map_layers_all_.insert(layer); -// } -// } -// } -// } -// // Callback funtion. -// // It publishes to specific topics. -// auto cb = [this, indices](const ros::TimerEvent&) { -// for (int i : indices) { -// publishMapOfIndex(i); -// } -// }; -// double duration = 1.0 / (fps + 0.00001); -// mapTimers_.push_back(nh_.createTimer(ros::Duration(duration), cb)); -// } -// } - -// void ElevationMappingNode::publishMapOfIndex(int index) { -// // publish the map layers of index -// if (!isGridmapUpdated_) { -// return; -// } -// grid_map_msgs::GridMap msg; -// std::vector layers; - -// { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in -// // map_layers_all_ -// std::lock_guard lock(mapMutex_); -// for (const auto& layer : map_layers_[index]) { -// const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); -// if (is_layer_in_all && gridMap_.exists(layer)) { -// layers.push_back(layer); -// } else if (map_.exists_layer(layer)) { -// // if there are layers which is not in the syncing layer. -// ElevationMappingWrapper::RowMatrixXf map_data; -// map_.get_layer_data(layer, map_data); -// gridMap_.add(layer, map_data); -// layers.push_back(layer); -// } -// } -// if (layers.empty()) { -// return; -// } +void ElevationMappingNode::setupMapPublishers() { + // Find the layers with highest fps. + float max_fps = -1; + // Create timers for each unique map frequencies + for (auto fps : map_fps_unique_) { + // Which publisher to call in the timer callback + std::vector indices; + // If this fps is max, update the map layers. + if (fps >= max_fps) { + max_fps = fps; + map_layers_all_.clear(); + } + for (int i = 0; i < map_fps_.size(); i++) { + if (map_fps_[i] == fps) { + indices.push_back(i); + // If this fps is max, add layers + if (fps >= max_fps) { + for (const auto layer : map_layers_[i]) { + map_layers_all_.insert(layer); + } + } + } + } + // Callback funtion. + // It publishes to specific topics. + auto cb = [this, indices]() -> void { + for (int i : indices) { + publishMapOfIndex(i); + } + }; + double duration = 1.0 / (fps + 0.00001); + mapTimers_.push_back(this->create_wall_timer(std::chrono::duration(duration), cb)); + } +} -// grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg); -// } -// msg.basic_layers = map_basic_layers_[index]; -// mapPubs_[index].publish(msg); -// } +void ElevationMappingNode::publishMapOfIndex(int index) { + // publish the map layers of index + if (!isGridmapUpdated_) { + return; + } + grid_map_msgs::msg::GridMap msg; + std::vector layers; + + // { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in + // // map_layers_all_ + // std::lock_guard lock(mapMutex_); + // for (const auto& layer : map_layers_[index]) { + // const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); + // if (is_layer_in_all && gridMap_.exists(layer)) { + // layers.push_back(layer); + // } else if (map_.exists_layer(layer)) { + // // if there are layers which is not in the syncing layer. + // ElevationMappingWrapper::RowMatrixXf map_data; + // map_.get_layer_data(layer, map_data); + // gridMap_.add(layer, map_data); + // layers.push_back(layer); + // } + // } + // if (layers.empty()) { + // return; + // } + + // grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg); + // } + + msg.basic_layers = map_basic_layers_[index]; + mapPubs_[index]->publish(msg); +} void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key) { @@ -487,6 +502,7 @@ void ElevationMappingNode::imageCallback(const std::shared_ptrnow(); // inputImage(image_msg, camera_info_msg, channels_[key]); RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageCallback processed an image in %f sec.", (this->now() - start).seconds()); + } diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index f0796abb..013fb1b9 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -18,12 +18,17 @@ #include + namespace elevation_mapping_cupy { -ElevationMappingWrapper::ElevationMappingWrapper(rclcpp::Node::SharedPtr node) -:node_(node) {} +ElevationMappingWrapper::ElevationMappingWrapper(){} -void ElevationMappingWrapper::initialize() { +void ElevationMappingWrapper::initialize(const std::shared_ptr& node){ + if (!node) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Invalid node shared pointer"); + return; + } + node_ = node; // Add the elevation_mapping_cupy path to sys.path auto threading = py::module::import("threading"); py::gil_scoped_acquire acquire; @@ -35,100 +40,124 @@ void ElevationMappingWrapper::initialize() { path.attr("insert")(0, module_path); auto elevation_mapping = py::module::import("elevation_mapping_cupy.elevation_mapping"); - // auto parameter = py::module::import("math"); - - // auto param_2 = parameter.attr("Parameter")(); - // setParameters(param_v2); - auto params = setParameters(); - - // map_ = elevation_mapping.attr("ElevationMap")(param_v2); + auto parameter = py::module::import("elevation_mapping_cupy.parameter"); + param_ = parameter.attr("Parameter")(); + setParameters(); + map_ = elevation_mapping.attr("ElevationMap")(param_); } // /** // * Load ros parameters into Parameter class. // * Search for the same name within the name space. // */ -py::object ElevationMappingWrapper::setParameters() { - auto parameter = py::module::import("elevation_mapping_cupy.parameter"); - auto param_2 = parameter.attr("Parameter")(); - return param_2; +void ElevationMappingWrapper::setParameters() { + + // auto parameter = py::module::import("elevation_mapping_cupy.parameter"); + // auto param_ = parameter.attr("Parameter")(); + // Get all parameters names and types. - // py::list paramNames = param_.attr("get_names")(); - // py::list paramTypes = param_.attr("get_types")(); - // py::gil_scoped_acquire acquire; + py::list paramNames = param_.attr("get_names")(); + py::list paramTypes = param_.attr("get_types")(); + py::gil_scoped_acquire acquire; // // Try to find the parameter in the ROS parameter server. // // If there was a parameter, set it to the Parameter variable. - // for (size_t i = 0; i < paramNames.size(); i++) { - // std::string type = py::cast(paramTypes[i]); - // std::string name = py::cast(paramNames[i]); - // if (type == "float") { - // float param; - // if (node_->get_parameter(name, param)) { - // param_.attr("set_value")(name, param); - // } - // } else if (type == "str") { - // std::string param; - // if (node_->get_parameter(name, param)) { - // param_.attr("set_value")(name, param); - // } - // } else if (type == "bool") { - // bool param; - // if (node_->get_parameter(name, param)) { - // param_.attr("set_value")(name, param); - // } - // } else if (type == "int") { - // int param; - // if (node_->get_parameter(name, param)) { - // param_.attr("set_value")(name, param); - // } - // } - // } + for (size_t i = 0; i < paramNames.size(); i++) { + std::string type = py::cast(paramTypes[i]); + std::string name = py::cast(paramNames[i]); + RCLCPP_INFO(node_->get_logger(), "type: %s, name %s", type.c_str(), name.c_str()); + if (type == "float") { + float param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %f", name.c_str(), param); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %f", name.c_str(), param); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } + } else if (type == "str") { + std::string param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %s", name.c_str(), param.c_str()); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %s", name.c_str(), param.c_str()); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } + } else if (type == "bool") { + bool param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %s", name.c_str(), param ? "true" : "false"); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %s", name.c_str(), param ? "true" : "false"); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } + } else if (type == "int") { + int param; + if (node_->get_parameter(name, param)) { + RCLCPP_INFO(node_->get_logger(), "Retrieved parameter: %s value: %d", name.c_str(), param); + param_.attr("set_value")(name, param); + RCLCPP_INFO(node_->get_logger(), "Set parameter: %s value: %d", name.c_str(), param); + } else { + RCLCPP_WARN(node_->get_logger(), "Parameter not found or invalid: %s", name.c_str()); + } + } + + } + py::dict sub_dict; + // rclcpp::Parameter subscribers; + std::vector parameter_prefixes; + auto parameters = node_->list_parameters(parameter_prefixes, 2); // List all parameters with a maximum depth of 10 + for (const auto& param_name : parameters.names) { + if (param_name.find("subscribers") != std::string::npos) { + RCLCPP_WARN(node_->get_logger(), "Subscriber : %s", param_name.c_str()); - // rclcpp::Parameter subscribers; - // node_->get_parameter("subscribers", subscribers); - - // py::dict sub_dict; - // auto subscribers_array = subscribers.as_string_array(); - // for (const auto& subscriber : subscribers_array) { - // const char* const name = subscriber.c_str(); - // if (!sub_dict.contains(name)) { - // sub_dict[name] = py::dict(); - // } - // std::map subscriber_params; - // node_->get_parameters(name, subscriber_params); - // for (const auto& param_pair : subscriber_params) { - // const char* const param_name = param_pair.first.c_str(); - // const auto& param_value = param_pair.second; - // std::vector arr; - // switch (param_value.get_type()) { - // case rclcpp::ParameterType::PARAMETER_STRING: - // sub_dict[name][param_name] = param_value.as_string(); - // break; - // case rclcpp::ParameterType::PARAMETER_INTEGER: - // sub_dict[name][param_name] = param_value.as_int(); - // break; - // case rclcpp::ParameterType::PARAMETER_DOUBLE: - // sub_dict[name][param_name] = param_value.as_double(); - // break; - // case rclcpp::ParameterType::PARAMETER_BOOL: - // sub_dict[name][param_name] = param_value.as_bool(); - // break; - // case rclcpp::ParameterType::PARAMETER_STRING_ARRAY: - // for (const auto& elem : param_value.as_string_array()) { - // arr.push_back(elem); - // } - // sub_dict[name][param_name] = arr; - // arr.clear(); - // break; - // default: - // sub_dict[name][param_name] = py::cast(param_value); - // break; - // } - // } - // } - // param_.attr("subscriber_cfg") = sub_dict; + + // const char* const name = param_name.c_str(); + // std::string subscriber_params; + // node_->get_parameter(param_name, subscriber); + // const auto& subscriber_params = subscriber.second; + // if (!sub_dict.contains(name)) { + // sub_dict[name] = py::dict(); + // } + // for (auto iterat : subscriber_params) { + // const char* const key = iterat.first.c_str(); + // const auto val = iterat.second; + // std::vector arr; + // switch (val.getType()) { + // case XmlRpc::XmlRpcValue::TypeString: + // sub_dict[name][key] = static_cast(val); + // break; + // case XmlRpc::XmlRpcValue::TypeInt: + // sub_dict[name][key] = static_cast(val); + // break; + // case XmlRpc::XmlRpcValue::TypeDouble: + // sub_dict[name][key] = static_cast(val); + // break; + // case XmlRpc::XmlRpcValue::TypeBoolean: + // sub_dict[name][key] = static_cast(val); + // break; + // case XmlRpc::XmlRpcValue::TypeArray: + // for (int32_t i = 0; i < val.size(); ++i) { + // auto elem = static_cast(val[i]); + // arr.push_back(elem); + // } + // sub_dict[name][key] = arr; + // arr.clear(); + // break; + // case XmlRpc::XmlRpcValue::TypeStruct: + // break; + // default: + // sub_dict[name][key] = py::cast(val); + // break; + // } + // } + } + } + param_.attr("subscriber_cfg") = sub_dict; + // // point cloud channel fusion From 61e10c55515e5300f6d2a27ac80ce0914c1e7780 Mon Sep 17 00:00:00 2001 From: amilearning Date: Thu, 7 Nov 2024 11:56:34 +0100 Subject: [PATCH 04/14] upto init --- elevation_mapping_cupy/CMakeLists.txt | 6 +- .../config/core/core_param.yaml | 5 +- .../elevation_mapping_wrapper.hpp | 2 + .../elevation_mapping.py | 38 ++-- .../elevation_mapping_cupy/parameter.py | 4 +- .../src/elevation_mapping_ros.cpp | 127 +++++------- .../src/elevation_mapping_wrapper.cpp | 191 +++++++++--------- 7 files changed, 183 insertions(+), 190 deletions(-) diff --git a/elevation_mapping_cupy/CMakeLists.txt b/elevation_mapping_cupy/CMakeLists.txt index fa8e2d25..91e4db8d 100644 --- a/elevation_mapping_cupy/CMakeLists.txt +++ b/elevation_mapping_cupy/CMakeLists.txt @@ -41,6 +41,10 @@ find_package(tf2_eigen REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(python_cmake_module REQUIRED) + +_ament_cmake_python_register_environment_hook() +ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR script/${PROJECT_NAME}) + # List dependencies for ament_target_dependencies set(dependencies rclcpp @@ -112,7 +116,7 @@ install( ) -ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR script/${PROJECT_NAME}) + install(PROGRAMS DESTINATION lib/${PROJECT_NAME} diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 4c0d8302..19f8aa28 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -64,11 +64,12 @@ elevation_mapping: enable_pointcloud_publishing: false enable_drift_corrected_TF_publishing: false enable_normal_color: false # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. + enable_normal: false #### Traversability filter ######## use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. - weight_file: '/config/core/weights.dat' # Weight file for traversability filter - + weight_file: '/share/elevation_mapping_cupy/config/core/weights.dat' # Weight file for traversability filter + plugin_config_file: '/share/elevation_mapping_cupy/config/core/plugin_config.yaml' # Configuration file for the plugin. #### Upper bound ######## use_only_above_for_upper_bound: false diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp index a6514843..f0090254 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_wrapper.hpp @@ -37,6 +37,8 @@ namespace py = pybind11; namespace elevation_mapping_cupy { +std::vector extract_unique_names(const std::map& subscriber_params); + class ElevationMappingWrapper { public: using RowMatrixXd = Eigen::Matrix; diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py index b0b0b40f..ea19064c 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -5,6 +5,8 @@ import os from typing import List, Any, Tuple, Union +from rclpy.logging import get_logger +from ament_index_python.packages import get_package_prefix import numpy as np import threading import subprocess @@ -42,6 +44,7 @@ import cupy as cp + xp = cp pool = cp.cuda.MemoryPool(cp.cuda.malloc_managed) cp.cuda.set_allocator(pool.malloc) @@ -97,26 +100,31 @@ def __init__(self, param: Parameter): self.compile_kernels() - + get_logger('elevation_mapping').info("Finished compiling kernels.") self.compile_image_kernels() + get_logger('elevation_mapping').info("Finished compiling image kernels.") + self.semantic_map.initialize_fusion() + get_logger('elevation_mapping').info("Finished compiling semantic_map kernels.") - # self.semantic_map.initialize_fusion() - - # weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') - # param.load_weights(weight_file) + weight_file = subprocess.getoutput('echo "' + param.weight_file + '"') + package_prefix = get_package_prefix('elevation_mapping_cupy') + get_logger('elevation_mapping').info("weight file : ." + str(package_prefix) + str(weight_file)) + param.load_weights(package_prefix+weight_file) - # if param.use_chainer: - # self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) - # else: - # self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) - # self.untraversable_polygon = xp.zeros((1, 2)) + if param.use_chainer: + self.traversability_filter = get_filter_chainer(param.w1, param.w2, param.w3, param.w_out) + else: + self.traversability_filter = get_filter_torch(param.w1, param.w2, param.w3, param.w_out) + + self.untraversable_polygon = xp.zeros((1, 2)) - # # Plugins - # self.plugin_manager = PluginManager(cell_n=self.cell_n) - # plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') - # self.plugin_manager.load_plugin_settings(plugin_config_file) + # Plugins + self.plugin_manager = PluginManager(cell_n=self.cell_n) + plugin_config_file = subprocess.getoutput('echo "' + param.plugin_config_file + '"') + get_logger('elevation_mapping').info("plugin file : ." + str(package_prefix) + str(plugin_config_file)) + self.plugin_manager.load_plugin_settings(package_prefix+plugin_config_file) - # self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") + self.map_initializer = MapInitializer(self.initial_variance, param.initialized_variance, xp=cp, method="points") def clear(self): """Reset all the layers of the elevation & the semantic map.""" diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py index 2193ad02..d23711eb 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/parameter.py @@ -210,7 +210,7 @@ class Parameter(Serializable): position_noise_thresh: float = 0.1 # if the position change is bigger than this value, the drift compensation happens. orientation_noise_thresh: float = 0.1 # if the orientation change is bigger than this value, the drift compensation happens. - # plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin + plugin_config_file: str = "config/plugin_config.yaml" # configuration file for the plugin weight_file: str = "config/weights.dat" # weight file for traversability filter initial_variance: float = 10.0 # initial variance for each cell. @@ -225,7 +225,7 @@ class Parameter(Serializable): cell_n: int = None # number of cells in the map true_cell_n: int = None # true number of cells in the map - def load_weights(self, filename): + def load_weights(self, filename: str): """ Load weights from a file into the model's parameters. diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 760a6dc5..7ebb5dd2 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -21,17 +21,6 @@ namespace elevation_mapping_cupy { -std::vector extract_unique_names(const std::map& subscriber_params) { - std::set unique_names_set; - for (const auto& param : subscriber_params) { - std::size_t pos = param.first.find('.'); - if (pos != std::string::npos) { - std::string name = param.first.substr(0, pos); - unique_names_set.insert(name); - } - } - return std::vector(unique_names_set.begin(), unique_names_set.end()); -} ElevationMappingNode::ElevationMappingNode() @@ -59,9 +48,6 @@ ElevationMappingNode::ElevationMappingNode() bool enablePointCloudPublishing(false); py::gil_scoped_acquire acquire; - auto math = py::module::import("math"); - double root_two = math.attr("sqrt")(2.0).cast(); - RCLCPP_INFO(this->get_logger(), "The square root of 2 is: %f", root_two); this->get_parameter("initialize_frame_id", initialize_frame_id_); this->get_parameter("initialize_tf_offset", initialize_tf_offset_); @@ -109,8 +95,8 @@ ElevationMappingNode::ElevationMappingNode() enablePointCloudPublishing_ = enablePointCloudPublishing; - // map_ = std::make_shared(); - // map_->initialize(node_); + map_ = std::make_shared(); + map_->initialize(node_); @@ -190,46 +176,38 @@ ElevationMappingNode::ElevationMappingNode() auto unique_pub_names = extract_unique_names(publisher_params); - // for (const auto& pub_name : unique_pub_names) { - // std::string topic_name = pub_name; - // std::vector layers_list; - // std::vector basic_layers_list; - // double fps; - // this->get_parameter("publishers." + pub_name + ".layers", layers_list) - // this->get_parameter("publishers." + pub_name + ".basic_layers", basic_layers_list) - // this->get_parameter("publishers." + pub_name + ".fps", fps) - // } + for (const auto& pub_name : unique_pub_names) { - std::string topic_name = pub_name; - double fps; - std::vector layers_list; - std::vector basic_layers_list; - - this->get_parameter("publishers." + pub_name + ".layers", layers_list); - this->get_parameter("publishers." + pub_name + ".basic_layers", basic_layers_list); - this->get_parameter("publishers." + pub_name + ".fps", fps); - - if (fps > updateGridMapFps) { - RCLCPP_WARN( - this->get_logger(), - "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " - "fps.", - topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); - } + std::string topic_name = pub_name; + double fps; + std::vector layers_list; + std::vector basic_layers_list; + + this->get_parameter("publishers." + pub_name + ".layers", layers_list); + this->get_parameter("publishers." + pub_name + ".basic_layers", basic_layers_list); + this->get_parameter("publishers." + pub_name + ".fps", fps); + + if (fps > updateGridMapFps) { + RCLCPP_WARN( + this->get_logger(), + "[ElevationMappingCupy] fps for topic %s is larger than map_acquire_fps (%f > %f). The topic data will be only updated at %f " + "fps.", + topic_name.c_str(), fps, updateGridMapFps, updateGridMapFps); + } - // Make publishers - auto pub = this->create_publisher(topic_name, 1); - RCLCPP_INFO(this->get_logger(), "Publishing map to topic %s", topic_name.c_str()); - mapPubs_.push_back(pub); + // Make publishers + auto pub = this->create_publisher(topic_name, 1); + RCLCPP_INFO(this->get_logger(), "Publishing map to topic %s", topic_name.c_str()); + mapPubs_.push_back(pub); - // Register map layers - map_layers_.push_back(layers_list); - map_basic_layers_.push_back(basic_layers_list); + // Register map layers + map_layers_.push_back(layers_list); + map_basic_layers_.push_back(basic_layers_list); - // Register map fps - map_fps_.push_back(fps); - map_fps_unique_.insert(fps); + // Register map fps + map_fps_.push_back(fps); + map_fps_unique_.insert(fps); } @@ -319,31 +297,36 @@ void ElevationMappingNode::publishMapOfIndex(int index) { if (!isGridmapUpdated_) { return; } + + + std::unique_ptr msg_ptr; grid_map_msgs::msg::GridMap msg; + std::vector layers; - // { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in - // // map_layers_all_ - // std::lock_guard lock(mapMutex_); - // for (const auto& layer : map_layers_[index]) { - // const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); - // if (is_layer_in_all && gridMap_.exists(layer)) { - // layers.push_back(layer); - // } else if (map_.exists_layer(layer)) { - // // if there are layers which is not in the syncing layer. - // ElevationMappingWrapper::RowMatrixXf map_data; - // map_.get_layer_data(layer, map_data); - // gridMap_.add(layer, map_data); - // layers.push_back(layer); - // } - // } - // if (layers.empty()) { - // return; - // } - - // grid_map::GridMapRosConverter::toMessage(gridMap_, layers, msg); - // } + { // need continuous lock between adding layers and converting to message. Otherwise updateGridmap can reset the data not in + // map_layers_all_ + std::lock_guard lock(mapMutex_); + for (const auto& layer : map_layers_[index]) { + const bool is_layer_in_all = map_layers_all_.find(layer) != map_layers_all_.end(); + if (is_layer_in_all && gridMap_.exists(layer)) { + layers.push_back(layer); + } else if (map_->exists_layer(layer)) { + // if there are layers which is not in the syncing layer. + ElevationMappingWrapper::RowMatrixXf map_data; + map_->get_layer_data(layer, map_data); + gridMap_.add(layer, map_data); + layers.push_back(layer); + } + } + if (layers.empty()) { + return; + } + msg_ptr = grid_map::GridMapRosConverter::toMessage(gridMap_, layers); + msg= *msg_ptr; + } + msg.basic_layers = map_basic_layers_[index]; mapPubs_[index]->publish(msg); } diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index 013fb1b9..0e9f29c5 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -21,6 +21,19 @@ namespace elevation_mapping_cupy { +std::vector extract_unique_names(const std::map& subscriber_params) { + std::set unique_names_set; + for (const auto& param : subscriber_params) { + std::size_t pos = param.first.find('.'); + if (pos != std::string::npos) { + std::string name = param.first.substr(0, pos); + unique_names_set.insert(name); + } + } + return std::vector(unique_names_set.begin(), unique_names_set.end()); +} + + ElevationMappingWrapper::ElevationMappingWrapper(){} void ElevationMappingWrapper::initialize(const std::shared_ptr& node){ @@ -41,9 +54,11 @@ void ElevationMappingWrapper::initialize(const std::shared_ptr& no auto elevation_mapping = py::module::import("elevation_mapping_cupy.elevation_mapping"); auto parameter = py::module::import("elevation_mapping_cupy.parameter"); - param_ = parameter.attr("Parameter")(); + param_ = parameter.attr("Parameter")(); setParameters(); map_ = elevation_mapping.attr("ElevationMap")(param_); + RCLCPP_INFO(node_->get_logger(), "ElevationMappingWrapper has been initialized"); + } // /** @@ -52,9 +67,6 @@ void ElevationMappingWrapper::initialize(const std::shared_ptr& no // */ void ElevationMappingWrapper::setParameters() { - // auto parameter = py::module::import("elevation_mapping_cupy.parameter"); - // auto param_ = parameter.attr("Parameter")(); - // Get all parameters names and types. py::list paramNames = param_.attr("get_names")(); py::list paramTypes = param_.attr("get_types")(); @@ -110,108 +122,91 @@ void ElevationMappingWrapper::setParameters() { // rclcpp::Parameter subscribers; std::vector parameter_prefixes; auto parameters = node_->list_parameters(parameter_prefixes, 2); // List all parameters with a maximum depth of 10 - for (const auto& param_name : parameters.names) { - if (param_name.find("subscribers") != std::string::npos) { - RCLCPP_WARN(node_->get_logger(), "Subscriber : %s", param_name.c_str()); - - // const char* const name = param_name.c_str(); - // std::string subscriber_params; - // node_->get_parameter(param_name, subscriber); - // const auto& subscriber_params = subscriber.second; - // if (!sub_dict.contains(name)) { - // sub_dict[name] = py::dict(); - // } - // for (auto iterat : subscriber_params) { - // const char* const key = iterat.first.c_str(); - // const auto val = iterat.second; - // std::vector arr; - // switch (val.getType()) { - // case XmlRpc::XmlRpcValue::TypeString: - // sub_dict[name][key] = static_cast(val); - // break; - // case XmlRpc::XmlRpcValue::TypeInt: - // sub_dict[name][key] = static_cast(val); - // break; - // case XmlRpc::XmlRpcValue::TypeDouble: - // sub_dict[name][key] = static_cast(val); - // break; - // case XmlRpc::XmlRpcValue::TypeBoolean: - // sub_dict[name][key] = static_cast(val); - // break; - // case XmlRpc::XmlRpcValue::TypeArray: - // for (int32_t i = 0; i < val.size(); ++i) { - // auto elem = static_cast(val[i]); - // arr.push_back(elem); - // } - // sub_dict[name][key] = arr; - // arr.clear(); - // break; - // case XmlRpc::XmlRpcValue::TypeStruct: - // break; - // default: - // sub_dict[name][key] = py::cast(val); - // break; - // } - // } + + std::map subscriber_params; + if (!node_->get_parameters("subscribers", subscriber_params)) { + RCLCPP_FATAL(node_->get_logger(), "There aren't any subscribers to be configured, the elevation mapping cannot be configured. Exit"); + rclcpp::shutdown(); + } + auto unique_sub_names = extract_unique_names(subscriber_params); + for (const auto& name : unique_sub_names) { + const char* const name_c = name.c_str(); + if (!sub_dict.contains(name_c)) { + sub_dict[name_c] = py::dict(); + } + std::string topic_name; + if(node_->get_parameter("subscribers." + name + ".topic_name", topic_name)){ + const char* topic_name_cstr = "topic_name"; + sub_dict[name_c][topic_name_cstr] = static_cast(topic_name); + std::string data_type; + if(node_->get_parameter("subscribers." + name + ".data_type", data_type)){ + const char* data_type_cstr = "data_type"; + sub_dict[name_c][data_type_cstr] = static_cast(data_type); + } + std::string info_name; + if(node_->get_parameter("subscribers." + name + ".data_type", info_name)){ + const char* info_name_cstr = "info_name"; + sub_dict[name_c][info_name_cstr] = static_cast(info_name); + } + std::string channel_name; + if(node_->get_parameter("subscribers." + name + ".data_type", channel_name)){ + const char* channel_name_cstr = "channel_name"; + sub_dict[name_c][channel_name_cstr] = static_cast(channel_name); } } + } + + + param_.attr("subscriber_cfg") = sub_dict; + // point cloud channel fusion + std::map pointcloud_channel_fusions_params; + if (node_->get_parameters("pointcloud_channel_fusions", pointcloud_channel_fusions_params)) { + py::dict pointcloud_channel_fusion_dict; + for (const auto& param : pointcloud_channel_fusions_params) { + std::string param_name = param.first; + std::string param_value = param.second.as_string(); + // Extract the string after "pointcloud_channel_fusions." + pointcloud_channel_fusion_dict[param_name.c_str()] = param_value; + } + // Print the dictionary for debugging + for (auto item : pointcloud_channel_fusion_dict) { + RCLCPP_INFO(node_->get_logger(), "pointcloud_channel_fusions Key: %s, Value: %s", std::string(py::str(item.first)).c_str(), std::string(py::str(item.second)).c_str()); + } + } else { + RCLCPP_WARN(node_->get_logger(), "No parameters found for 'pointcloud_channel_fusions'"); + } - // // point cloud channel fusion - // if (!node_->has_parameter("pointcloud_channel_fusions")) { - // RCLCPP_WARN(node_->get_logger(), "No pointcloud_channel_fusions parameter found. Using default values."); - // } else { - // rclcpp::Parameter pointcloud_channel_fusion; - // node_->get_parameter("pointcloud_channel_fusions", pointcloud_channel_fusion); - - // py::dict pointcloud_channel_fusion_dict; - // auto pointcloud_channel_fusion_map = pointcloud_channel_fusion.as_string_array(); - // for (const auto& channel_fusion : pointcloud_channel_fusion_map) { - // const char* const fusion_name = channel_fusion.c_str(); - // std::string fusion; - // node_->get_parameter(fusion_name, fusion); - // if (!pointcloud_channel_fusion_dict.contains(fusion_name)) { - // pointcloud_channel_fusion_dict[fusion_name] = fusion; - // } - // } - // RCLCPP_INFO_STREAM(node_->get_logger(), "pointcloud_channel_fusion_dict: " << pointcloud_channel_fusion_dict); - // param_.attr("pointcloud_channel_fusions") = pointcloud_channel_fusion_dict; - // } + // image channel fusion + std::map image_channel_fusions_params; + if (node_->get_parameters("image_channel_fusions", image_channel_fusions_params)) { + py::dict image_channel_fusion_dict; + for (const auto& param : image_channel_fusions_params) { + std::string param_name = param.first; + std::string param_value = param.second.as_string(); + // Extract the string after "pointcloud_channel_fusions." + image_channel_fusion_dict[param_name.c_str()] = param_value; + } + // Print the dictionary for debugging + for (auto item : image_channel_fusion_dict) { + RCLCPP_INFO(node_->get_logger(), "image_channel_fusions Key: %s, Value: %s", std::string(py::str(item.first)).c_str(), std::string(py::str(item.second)).c_str()); + } + } else { + RCLCPP_WARN(node_->get_logger(), "No parameters found for 'image_channel_fusions'"); + } - - // // image channel fusion - // if (!node_->has_parameter("image_channel_fusions")) { - // RCLCPP_WARN(node_->get_logger(), "No image_channel_fusions parameter found. Using default values."); - // } else { - // rclcpp::Parameter image_channel_fusion; - // node_->get_parameter("image_channel_fusions", image_channel_fusion); - - // py::dict image_channel_fusion_dict; - // auto image_channel_fusion_map = image_channel_fusion.as_string_array(); - // for (const auto& channel_fusion : image_channel_fusion_map) { - // const char* const channel_fusion_name = channel_fusion.c_str(); - // std::string fusion; - // node_->get_parameter(channel_fusion_name, fusion); - // if (!image_channel_fusion_dict.contains(channel_fusion_name)) { - // image_channel_fusion_dict[channel_fusion_name] = fusion; - // } - // } - // RCLCPP_INFO_STREAM(node_->get_logger(), "image_channel_fusion_dict: " << image_channel_fusion_dict); - // param_.attr("image_channel_fusions") = image_channel_fusion_dict; - // } - - // param_.attr("update")(); - // resolution_ = py::cast(param_.attr("get_value")("resolution")); - // map_length_ = py::cast(param_.attr("get_value")("true_map_length")); - // map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); - - // node_->declare_parameter("enable_normal", false); - // node_->declare_parameter("enable_normal_color", false); - // enable_normal_ = node_->get_parameter("enable_normal").as_bool(); - // enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool(); + + param_.attr("update")(); + resolution_ = py::cast(param_.attr("get_value")("resolution")); + map_length_ = py::cast(param_.attr("get_value")("true_map_length")); + map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); + + + enable_normal_ = node_->get_parameter("enable_normal").as_bool(); + enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool(); } From 50d1623164d5e6e02f8e21a4038289397bd93a47 Mon Sep 17 00:00:00 2001 From: amilearning Date: Thu, 7 Nov 2024 14:49:09 +0100 Subject: [PATCH 05/14] upto initializationMap --- elevation_map_msgs/CMakeLists.txt | 2 +- elevation_mapping_cupy/CMakeLists.txt | 2 +- .../elevation_mapping_ros.hpp | 31 +- .../src/elevation_mapping_ros.cpp | 366 ++++++++++-------- 4 files changed, 227 insertions(+), 174 deletions(-) diff --git a/elevation_map_msgs/CMakeLists.txt b/elevation_map_msgs/CMakeLists.txt index 0ea3f7cb..08e4467b 100644 --- a/elevation_map_msgs/CMakeLists.txt +++ b/elevation_map_msgs/CMakeLists.txt @@ -19,7 +19,7 @@ set(srv_files rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} - DEPENDENCIES geometry_msgs std_msgs # Added std_msgs + DEPENDENCIES geometry_msgs std_msgs std_msgs # Added std_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/elevation_mapping_cupy/CMakeLists.txt b/elevation_mapping_cupy/CMakeLists.txt index 91e4db8d..9925bc5d 100644 --- a/elevation_mapping_cupy/CMakeLists.txt +++ b/elevation_mapping_cupy/CMakeLists.txt @@ -65,7 +65,7 @@ set(dependencies # Include directories include_directories( - include + include ${PYTHON_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index b4abac57..f887afe4 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -26,8 +26,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -56,6 +56,7 @@ #include #include #include +#include #include "elevation_mapping_cupy/elevation_mapping_wrapper.hpp" @@ -108,19 +109,29 @@ void imageChannelCallback(const std::shared_ptr& // void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); // // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); // void publishAsPointCloud(const grid_map::GridMap& map) const; -// bool getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); +bool getSubmap( const std::shared_ptr request, std::shared_ptr response); + + // bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); -// bool initializeMap(elevation_map_msgs::Initialize::Request& request, elevation_map_msgs::Initialize::Response& response); -// bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); -// bool clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + + void initializeMap(const std::shared_ptr request, std::shared_ptr response); + + + // bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); + + +void clearMap(const std::shared_ptr request, std::shared_ptr response); +void clearMapWithInitializer(const std::shared_ptr request, std::shared_ptr response); + + // bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); // void updatePose(const ros::TimerEvent&); // void updateVariance(const ros::TimerEvent&); // void updateTime(const ros::TimerEvent&); // void updateGridMap(const ros::TimerEvent&); // void publishNormalAsArrow(const grid_map::GridMap& map) const; -// void initializeWithTF(); -// void publishMapToOdom(double error); + void initializeWithTF(); + void publishMapToOdom(double error); // void publishStatistics(const ros::TimerEvent&); void publishMapOfIndex(int index); @@ -143,10 +154,10 @@ void imageChannelCallback(const std::shared_ptr& std::shared_ptr tfBuffer_; - rclcpp::Publisher::SharedPtr alivePub_; + rclcpp::Publisher::SharedPtr alivePub_; rclcpp::Publisher::SharedPtr pointPub_; - rclcpp::Publisher::SharedPtr normalPub_; - rclcpp::Publisher::SharedPtr statisticsPub_; + rclcpp::Publisher::SharedPtr normalPub_; + rclcpp::Publisher::SharedPtr statisticsPub_; rclcpp::Service::SharedPtr rawSubmapService_; rclcpp::Service::SharedPtr clearMapService_; rclcpp::Service::SharedPtr clearMapWithInitializerService_; diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 7ebb5dd2..0f29da25 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -214,19 +214,32 @@ ElevationMappingNode::ElevationMappingNode() setupMapPublishers(); -// pointPub_ = nh_.advertise("elevation_map_points", 1); -// alivePub_ = nh_.advertise("alive", 1); -// normalPub_ = nh_.advertise("normal", 1); -// statisticsPub_ = nh_.advertise("statistics", 1); - -// gridMap_.setFrameId(mapFrameId_); -// rawSubmapService_ = nh_.advertiseService("get_raw_submap", &ElevationMappingNode::getSubmap, this); -// clearMapService_ = nh_.advertiseService("clear_map", &ElevationMappingNode::clearMap, this); -// initializeMapService_ = nh_.advertiseService("initialize", &ElevationMappingNode::initializeMap, this); -// clearMapWithInitializerService_ = -// nh_.advertiseService("clear_map_with_initializer", &ElevationMappingNode::clearMapWithInitializer, this); -// setPublishPointService_ = nh_.advertiseService("set_publish_points", &ElevationMappingNode::setPublishPoint, this); -// checkSafetyService_ = nh_.advertiseService("check_safety", &ElevationMappingNode::checkSafety, this); + pointPub_ = this->create_publisher("elevation_map_points", 1); + alivePub_ = this->create_publisher("alive", 1); + normalPub_ = this->create_publisher("normal", 1); + statisticsPub_ = this->create_publisher("statistics", 1); + + gridMap_.setFrameId(mapFrameId_); + +rawSubmapService_ = this->create_service( + "get_raw_submap", std::bind(&ElevationMappingNode::getSubmap, this, std::placeholders::_1, std::placeholders::_2)); + +clearMapService_ = this->create_service( + "clear_map", std::bind(&ElevationMappingNode::clearMap, this, std::placeholders::_1, std::placeholders::_2)); + +clearMapWithInitializerService_ = this->create_service( + "clear_map_with_initializer", std::bind(&ElevationMappingNode::clearMapWithInitializer, this, std::placeholders::_1, std::placeholders::_2)); + + +initializeMapService_ = this->create_service( + "initialize", std::bind(&ElevationMappingNode::initializeMap, this, std::placeholders::_1, std::placeholders::_2)); + + +// setPublishPointService_ = this->create_service( +// "set_publish_points", std::bind(&ElevationMappingNode::setPublishPoint, this, std::placeholders::_1, std::placeholders::_2)); +// checkSafetyService_ = this->create_service( +// "check_safety", std::bind(&ElevationMappingNode::checkSafety, this, std::placeholders::_1, std::placeholders::_2)); + // if (updateVarianceFps > 0) { // double duration = 1.0 / (updateVarianceFps + 0.00001); @@ -390,12 +403,12 @@ void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2:: orientationError = orientationError_; } - // map_.input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, - // orientationError); + map_->input(points, channels, transformationSensorToMap.rotation(), transformationSensorToMap.translation(), positionError, + orientationError); - // if (enableDriftCorrectedTFPublishing_) { - // publishMapToOdom(map_.get_additive_mean_error()); - // } + if (enableDriftCorrectedTFPublishing_) { + publishMapToOdom(map_->get_additive_mean_error()); + } RCLCPP_DEBUG(this->get_logger(), "ElevationMap processed a point cloud (%i points) in %f sec.", static_cast(points.size()), (this->now() - start).seconds()); @@ -540,101 +553,118 @@ RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an // pointPub_.publish(msg); // } -// bool ElevationMappingNode::getSubmap(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response) { -// std::string requestedFrameId = request.frame_id; -// Eigen::Isometry3d transformationOdomToMap; -// grid_map::Position requestedSubmapPosition(request.position_x, request.position_y); -// if (requestedFrameId != mapFrameId_) { -// tf::StampedTransform transformTf; -// const auto& timeStamp = ros::Time::now(); -// try { -// transformListener_.waitForTransform(requestedFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); -// transformListener_.lookupTransform(requestedFrameId, mapFrameId_, timeStamp, transformTf); -// tf::poseTFToEigen(transformTf, transformationOdomToMap); -// } catch (tf::TransformException& ex) { -// ROS_ERROR("%s", ex.what()); -// return false; -// } -// Eigen::Vector3d p(request.position_x, request.position_y, 0); -// Eigen::Vector3d mapP = transformationOdomToMap.inverse() * p; -// requestedSubmapPosition.x() = mapP.x(); -// requestedSubmapPosition.y() = mapP.y(); -// } -// grid_map::Length requestedSubmapLength(request.length_x, request.length_y); -// ROS_DEBUG("Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", requestedSubmapPosition.x(), requestedSubmapPosition.y(), -// requestedSubmapLength(0), requestedSubmapLength(1)); -// bool isSuccess; -// grid_map::Index index; -// grid_map::GridMap subMap; -// { -// std::lock_guard lock(mapMutex_); -// subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, index, isSuccess); -// } -// const auto& length = subMap.getLength(); -// if (requestedFrameId != mapFrameId_) { -// subMap = subMap.getTransformedMap(transformationOdomToMap, "elevation", requestedFrameId); -// } +bool ElevationMappingNode::getSubmap( + const std::shared_ptr request, + std::shared_ptr response) { + std::string requestedFrameId = request->frame_id; + Eigen::Isometry3d transformationOdomToMap; + geometry_msgs::msg::Pose pose; + grid_map::Position requestedSubmapPosition(request->position_x, request->position_y); + if (requestedFrameId != mapFrameId_) { + geometry_msgs::msg::TransformStamped transformStamped; + const auto& timeStamp = this->now(); + try { + transformStamped = tfBuffer_->lookupTransform(requestedFrameId, mapFrameId_, timeStamp, tf2::durationFromSec(1.0)); + + + pose.position.x = transformStamped.transform.translation.x; + pose.position.y = transformStamped.transform.translation.y; + pose.position.z = transformStamped.transform.translation.z; + pose.orientation = transformStamped.transform.rotation; -// if (request.layers.empty()) { -// grid_map::GridMapRosConverter::toMessage(subMap, response.map); -// } else { -// std::vector layers; -// for (const auto& layer : request.layers) { -// layers.push_back(layer); -// } -// grid_map::GridMapRosConverter::toMessage(subMap, layers, response.map); -// } -// return isSuccess; -// } + tf2::fromMsg(pose, transformationOdomToMap); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return false; + } + Eigen::Vector3d p(request->position_x, request->position_y, 0); + Eigen::Vector3d mapP = transformationOdomToMap.inverse() * p; + requestedSubmapPosition.x() = mapP.x(); + requestedSubmapPosition.y() = mapP.y(); + } + grid_map::Length requestedSubmapLength(request->length_x, request->length_y); + RCLCPP_DEBUG(this->get_logger(), "Elevation submap request: Position x=%f, y=%f, Length x=%f, y=%f.", + requestedSubmapPosition.x(), requestedSubmapPosition.y(), + requestedSubmapLength(0), requestedSubmapLength(1)); + + bool isSuccess; + grid_map::Index index; + grid_map::GridMap subMap; + { + std::lock_guard lock(mapMutex_); + subMap = gridMap_.getSubmap(requestedSubmapPosition, requestedSubmapLength, isSuccess); + } + const auto& length = subMap.getLength(); + if (requestedFrameId != mapFrameId_) { + subMap = subMap.getTransformedMap(transformationOdomToMap, "elevation", requestedFrameId); + } -// bool ElevationMappingNode::clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { -// ROS_INFO("Clearing map."); -// map_.clear(); -// if (alwaysClearWithInitializer_) { -// initializeWithTF(); -// } -// return true; -// } + if (request->layers.empty()) { + auto msg_ptr = grid_map::GridMapRosConverter::toMessage(subMap); + response->map = *msg_ptr; + } else { + std::vector layers; + for (const auto& layer : request->layers) { + layers.push_back(layer); + } + auto msg_ptr = grid_map::GridMapRosConverter::toMessage(subMap, layers); + response->map = *msg_ptr; -// bool ElevationMappingNode::clearMapWithInitializer(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { -// ROS_INFO("Clearing map with initializer."); -// map_.clear(); -// initializeWithTF(); -// return true; -// } + } + return isSuccess; +} -// void ElevationMappingNode::initializeWithTF() { -// std::vector points; -// const auto& timeStamp = ros::Time::now(); -// int i = 0; -// Eigen::Vector3d p; -// for (const auto& frame_id : initialize_frame_id_) { -// // Get tf from map frame to tf frame -// Eigen::Affine3d transformationBaseToMap; -// tf::StampedTransform transformTf; -// try { -// transformListener_.waitForTransform(mapFrameId_, frame_id, timeStamp, ros::Duration(1.0)); -// transformListener_.lookupTransform(mapFrameId_, frame_id, timeStamp, transformTf); -// poseTFToEigen(transformTf, transformationBaseToMap); -// } catch (tf::TransformException& ex) { -// ROS_ERROR("%s", ex.what()); -// return; -// } -// p = transformationBaseToMap.translation(); -// p.z() += initialize_tf_offset_[i]; -// points.push_back(p); -// i++; -// } -// if (!points.empty() && points.size() < 3) { -// points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, initializeTfGridSize_, 0)); -// points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, initializeTfGridSize_, 0)); -// points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, -initializeTfGridSize_, 0)); -// points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, -initializeTfGridSize_, 0)); -// } -// ROS_INFO_STREAM("Initializing map with points using " << initializeMethod_); -// map_.initializeWithPoints(points, initializeMethod_); -// } + + +void ElevationMappingNode::clearMap(const std::shared_ptr request, + std::shared_ptr response) { + + std::lock_guard lock(mapMutex_); + RCLCPP_INFO(this->get_logger(), "Clearing map."); + map_->clear(); + if (alwaysClearWithInitializer_) { + initializeWithTF(); + } +} + +void ElevationMappingNode::clearMapWithInitializer(const std::shared_ptr request, std::shared_ptr response){ + RCLCPP_INFO(this->get_logger(), "Clearing map with initializer"); + map_->clear(); + initializeWithTF(); + +} + +void ElevationMappingNode::initializeWithTF() { + std::vector points; + const auto& timeStamp = this->now(); + int i = 0; + Eigen::Vector3d p; + for (const auto& frame_id : initialize_frame_id_) { + // Get tf from map frame to tf frame + Eigen::Affine3d transformationBaseToMap; + geometry_msgs::msg::TransformStamped transformStamped; + try { + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, frame_id, timeStamp, tf2::durationFromSec(1.0)); + transformationBaseToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } + p = transformationBaseToMap.translation(); + p.z() += initialize_tf_offset_[i]; + points.push_back(p); + i++; + } + if (!points.empty() && points.size() < 3) { + points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, initializeTfGridSize_, 0)); + points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, initializeTfGridSize_, 0)); + points.emplace_back(p + Eigen::Vector3d(initializeTfGridSize_, -initializeTfGridSize_, 0)); + points.emplace_back(p + Eigen::Vector3d(-initializeTfGridSize_, -initializeTfGridSize_, 0)); + } + RCLCPP_INFO(this->get_logger(), "Initializing map with points using %s", initializeMethod_.c_str()); + map_->initializeWithPoints(points, initializeMethod_); +} // bool ElevationMappingNode::checkSafety(elevation_map_msgs::CheckSafety::Request& request, // elevation_map_msgs::CheckSafety::Response& response) { @@ -737,52 +767,51 @@ RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an // isGridmapUpdated_ = true; // } -// bool ElevationMappingNode::initializeMap(elevation_map_msgs::Initialize::Request& request, -// elevation_map_msgs::Initialize::Response& response) { -// // If initialize method is points -// if (request.type == request.POINTS) { -// std::vector points; -// for (const auto& point : request.points) { -// const auto& pointFrameId = point.header.frame_id; -// const auto& timeStamp = point.header.stamp; -// const auto& pvector = Eigen::Vector3d(point.point.x, point.point.y, point.point.z); - -// // Get tf from map frame to points' frame -// if (mapFrameId_ != pointFrameId) { -// Eigen::Affine3d transformationBaseToMap; -// tf::StampedTransform transformTf; -// try { -// transformListener_.waitForTransform(mapFrameId_, pointFrameId, timeStamp, ros::Duration(1.0)); -// transformListener_.lookupTransform(mapFrameId_, pointFrameId, timeStamp, transformTf); -// poseTFToEigen(transformTf, transformationBaseToMap); -// } catch (tf::TransformException& ex) { -// ROS_ERROR("%s", ex.what()); -// return false; -// } -// const auto transformed_p = transformationBaseToMap * pvector; -// points.push_back(transformed_p); -// } else { -// points.push_back(pvector); -// } -// } -// std::string method; -// switch (request.method) { -// case request.NEAREST: -// method = "nearest"; -// break; -// case request.LINEAR: -// method = "linear"; -// break; -// case request.CUBIC: -// method = "cubic"; -// break; -// } -// ROS_INFO_STREAM("Initializing map with points using " << method); -// map_.initializeWithPoints(points, method); -// } -// response.success = true; -// return true; -// } +void ElevationMappingNode::initializeMap(const std::shared_ptr request, + std::shared_ptr response) { + // If initialize method is points + if (request->type == elevation_map_msgs::srv::Initialize::Request::POINTS) { + std::vector points; + for (const auto& point : request->points) { + const auto& pointFrameId = point.header.frame_id; + const auto& timeStamp = point.header.stamp; + const auto& pvector = Eigen::Vector3d(point.point.x, point.point.y, point.point.z); + + // Get tf from map frame to points' frame + if (mapFrameId_ != pointFrameId) { + Eigen::Affine3d transformationBaseToMap; + geometry_msgs::msg::TransformStamped transformStamped; + try { + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, pointFrameId, timeStamp, tf2::durationFromSec(1.0)); + transformationBaseToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + response->success = false; + return; + } + const auto transformed_p = transformationBaseToMap * pvector; + points.push_back(transformed_p); + } else { + points.push_back(pvector); + } + } + std::string method; + switch (request->method) { + case elevation_map_msgs::srv::Initialize::Request::NEAREST: + method = "nearest"; + break; + case elevation_map_msgs::srv::Initialize::Request::LINEAR: + method = "linear"; + break; + case elevation_map_msgs::srv::Initialize::Request::CUBIC: + method = "cubic"; + break; + } + RCLCPP_INFO(this->get_logger(), "Initializing map with points using %s", method.c_str()); + map_->initializeWithPoints(points, method); + } + response->success = true; +} // void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const { // auto startTime = ros::Time::now(); @@ -844,13 +873,26 @@ RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an // return marker; // } -// void ElevationMappingNode::publishMapToOdom(double error) { -// tf::Transform transform; -// transform.setOrigin(tf::Vector3(0.0, 0.0, error)); -// tf::Quaternion q; -// q.setRPY(0, 0, 0); -// transform.setRotation(q); -// tfBroadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), mapFrameId_, correctedMapFrameId_)); -// } + +void ElevationMappingNode::publishMapToOdom(double error) { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = this->now(); + transform_stamped.header.frame_id = mapFrameId_; + transform_stamped.child_frame_id = correctedMapFrameId_; + transform_stamped.transform.translation.x = 0.0; + transform_stamped.transform.translation.y = 0.0; + transform_stamped.transform.translation.z = error; + + tf2::Quaternion q; + q.setRPY(0, 0, 0); + transform_stamped.transform.rotation.x = q.x(); + transform_stamped.transform.rotation.y = q.y(); + transform_stamped.transform.rotation.z = q.z(); + transform_stamped.transform.rotation.w = q.w(); + + tfBroadcaster_->sendTransform(transform_stamped); +} + + } // namespace elevation_mapping_cupy From f71c3c36f60430f8c6013f712a45ce3fc8583f1b Mon Sep 17 00:00:00 2001 From: amilearning Date: Fri, 8 Nov 2024 04:43:12 +0100 Subject: [PATCH 06/14] pointcloud done --- .../config/core/core_param.yaml | 8 +- .../elevation_mapping_ros.hpp | 35 +- .../src/elevation_mapping_ros.cpp | 325 +++++++++--------- 3 files changed, 192 insertions(+), 176 deletions(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 19f8aa28..c128291f 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,7 +1,7 @@ elevation_mapping: ros__parameters: #### Basic parameters ######## - cell_n: 10000 # number of cells in the map. + cell_n: 40000 # number of cells in the map. data_type: 'float32' # data type for the map. average_weight: 0.5 drift_compensation_variance_inlier: 0.1 @@ -9,8 +9,8 @@ elevation_mapping: min_filter_size: 5 # size of the filter for min filter. min_filter_iteration: 3 # number of iterations for min filter. initialized_variance: 10.0 # initial variance for each cell. - resolution: 0.04 # resolution in m. - map_length: 8.0 # map's size in m. + resolution: 0.1 # resolution in m. + map_length: 20.0 # map's size in m. sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). mahalanobis_thresh: 2.0 # points outside this distance is outlier. outlier_variance: 0.01 # if point is outlier, add this value to the cell. @@ -105,7 +105,7 @@ elevation_mapping: subscribers: pointcloud1: - topic_name: '/a200/sensors/camera_1/points' + topic_name: '/a200/sensors/camera_0/points' data_type: 'pointcloud' image1: topic_name: '/a200/sensors/camera_0/color/image' diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index f887afe4..7cf91ce1 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -108,34 +108,33 @@ void imageChannelCallback(const std::shared_ptr& // void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); // // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); -// void publishAsPointCloud(const grid_map::GridMap& map) const; +void publishAsPointCloud(const grid_map::GridMap& map) const; bool getSubmap( const std::shared_ptr request, std::shared_ptr response); // bool checkSafety(elevation_map_msgs::CheckSafety::Request& request, elevation_map_msgs::CheckSafety::Response& response); - void initializeMap(const std::shared_ptr request, std::shared_ptr response); - - - // bool clearMap(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); - - +void initializeMap(const std::shared_ptr request, std::shared_ptr response); void clearMap(const std::shared_ptr request, std::shared_ptr response); void clearMapWithInitializer(const std::shared_ptr request, std::shared_ptr response); -// bool setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response); -// void updatePose(const ros::TimerEvent&); -// void updateVariance(const ros::TimerEvent&); -// void updateTime(const ros::TimerEvent&); -// void updateGridMap(const ros::TimerEvent&); -// void publishNormalAsArrow(const grid_map::GridMap& map) const; - void initializeWithTF(); - void publishMapToOdom(double error); -// void publishStatistics(const ros::TimerEvent&); - void publishMapOfIndex(int index); -// visualization_msgs::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; +void setPublishPoint(const std::shared_ptr request, + std::shared_ptr response); + + +void updateVariance(); +void updateTime(); +void updatePose(); +void updateGridMap(); +void publishStatistics(); + +void publishNormalAsArrow(const grid_map::GridMap& map) const; +void initializeWithTF(); +void publishMapToOdom(double error); +void publishMapOfIndex(int index); +visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const int id) const; rclcpp::Node::SharedPtr node_; // image_transport::ImageTransport it_; diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 0f29da25..57f8bd83 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -234,35 +234,40 @@ clearMapWithInitializerService_ = this->create_service( initializeMapService_ = this->create_service( "initialize", std::bind(&ElevationMappingNode::initializeMap, this, std::placeholders::_1, std::placeholders::_2)); +setPublishPointService_ = this->create_service( + "set_publish_points", std::bind(&ElevationMappingNode::setPublishPoint, this, std::placeholders::_1, std::placeholders::_2)); -// setPublishPointService_ = this->create_service( -// "set_publish_points", std::bind(&ElevationMappingNode::setPublishPoint, this, std::placeholders::_1, std::placeholders::_2)); // checkSafetyService_ = this->create_service( // "check_safety", std::bind(&ElevationMappingNode::checkSafety, this, std::placeholders::_1, std::placeholders::_2)); -// if (updateVarianceFps > 0) { -// double duration = 1.0 / (updateVarianceFps + 0.00001); -// updateVarianceTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateVariance, this, false, true); -// } -// if (timeInterval > 0) { -// double duration = timeInterval; -// updateTimeTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateTime, this, false, true); -// } -// if (updatePoseFps > 0) { -// double duration = 1.0 / (updatePoseFps + 0.00001); -// updatePoseTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updatePose, this, false, true); -// } -// if (updateGridMapFps > 0) { -// double duration = 1.0 / (updateGridMapFps + 0.00001); -// updateGridMapTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::updateGridMap, this, false, true); -// } -// if (publishStatisticsFps > 0) { -// double duration = 1.0 / (publishStatisticsFps + 0.00001); -// publishStatisticsTimer_ = nh_.createTimer(ros::Duration(duration), &ElevationMappingNode::publishStatistics, this, false, true); -// } -// lastStatisticsPublishedTime_ = ros::Time::now(); -// ROS_INFO("[ElevationMappingCupy] finish initialization"); + if (updateVarianceFps > 0) { + double duration = 1.0 / (updateVarianceFps + 0.00001); + updateVarianceTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updateVariance, this)); + } + if (timeInterval > 0) { + double duration = timeInterval; + updateTimeTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updateTime, this)); + } + if (updatePoseFps > 0) { + double duration = 1.0 / (updatePoseFps + 0.00001); + updatePoseTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updatePose, this)); + } + if (updateGridMapFps > 0) { + double duration = 1.0 / (updateGridMapFps + 0.00001); + updateGridMapTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::updateGridMap, this)); + } + if (publishStatisticsFps > 0) { + double duration = 1.0 / (publishStatisticsFps + 0.00001); + publishStatisticsTimer_ = this->create_wall_timer(std::chrono::duration(duration), + std::bind(&ElevationMappingNode::publishStatistics, this)); + } + lastStatisticsPublishedTime_ = this->now(); + RCLCPP_INFO(this->get_logger(), "[ElevationMappingCupy] finish initialization"); } // namespace elevation_mapping_cupy @@ -513,45 +518,51 @@ auto start = this->now(); RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); } -// void ElevationMappingNode::updatePose(const ros::TimerEvent&) { -// tf::StampedTransform transformTf; -// const auto& timeStamp = ros::Time::now(); -// Eigen::Affine3d transformationBaseToMap; -// try { -// transformListener_.waitForTransform(mapFrameId_, baseFrameId_, timeStamp, ros::Duration(1.0)); -// transformListener_.lookupTransform(mapFrameId_, baseFrameId_, timeStamp, transformTf); -// poseTFToEigen(transformTf, transformationBaseToMap); -// } catch (tf::TransformException& ex) { -// ROS_ERROR("%s", ex.what()); -// return; -// } -// // This is to check if the robot is moving. If the robot is not moving, drift compensation is disabled to avoid creating artifacts. -// Eigen::Vector3d position(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); -// map_.move_to(position, transformationBaseToMap.rotation().transpose()); -// Eigen::Vector3d position3(transformTf.getOrigin().x(), transformTf.getOrigin().y(), transformTf.getOrigin().z()); -// Eigen::Vector4d orientation(transformTf.getRotation().x(), transformTf.getRotation().y(), transformTf.getRotation().z(), -// transformTf.getRotation().w()); -// lowpassPosition_ = positionAlpha_ * position3 + (1 - positionAlpha_) * lowpassPosition_; -// lowpassOrientation_ = orientationAlpha_ * orientation + (1 - orientationAlpha_) * lowpassOrientation_; -// { -// std::lock_guard lock(errorMutex_); -// positionError_ = (position3 - lowpassPosition_).norm(); -// orientationError_ = (orientation - lowpassOrientation_).norm(); -// } +void ElevationMappingNode::updatePose() { + geometry_msgs::msg::TransformStamped transformStamped; + const auto& timeStamp = this->now(); + Eigen::Affine3d transformationBaseToMap; + try { + transformStamped = tfBuffer_->lookupTransform(mapFrameId_, baseFrameId_, timeStamp, tf2::durationFromSec(1.0)); + transformationBaseToMap = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } -// if (useInitializerAtStart_) { -// ROS_INFO("Clearing map with initializer."); -// initializeWithTF(); -// useInitializerAtStart_ = false; -// } -// } + // This is to check if the robot is moving. If the robot is not moving, drift compensation is disabled to avoid creating artifacts. + Eigen::Vector3d position(transformStamped.transform.translation.x, + transformStamped.transform.translation.y, + transformStamped.transform.translation.z); + map_->move_to(position, transformationBaseToMap.rotation().transpose()); + Eigen::Vector3d position3(transformStamped.transform.translation.x, + transformStamped.transform.translation.y, + transformStamped.transform.translation.z); + Eigen::Vector4d orientation(transformStamped.transform.rotation.x, + transformStamped.transform.rotation.y, + transformStamped.transform.rotation.z, + transformStamped.transform.rotation.w); + lowpassPosition_ = positionAlpha_ * position3 + (1 - positionAlpha_) * lowpassPosition_; + lowpassOrientation_ = orientationAlpha_ * orientation + (1 - orientationAlpha_) * lowpassOrientation_; + { + std::lock_guard lock(errorMutex_); + positionError_ = (position3 - lowpassPosition_).norm(); + orientationError_ = (orientation - lowpassOrientation_).norm(); + } -// void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const { -// sensor_msgs::PointCloud2 msg; -// grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg); -// pointPub_.publish(msg); -// } + if (useInitializerAtStart_) { + RCLCPP_INFO(this->get_logger(), "Clearing map with initializer."); + initializeWithTF(); + useInitializerAtStart_ = false; + } +} + +void ElevationMappingNode::publishAsPointCloud(const grid_map::GridMap& map) const { + sensor_msgs::msg::PointCloud2 msg; + grid_map::GridMapRosConverter::toPointCloud(map, "elevation", msg); + pointPub_->publish(msg); +} bool ElevationMappingNode::getSubmap( @@ -723,49 +734,52 @@ void ElevationMappingNode::initializeWithTF() { // return true; // } -// bool ElevationMappingNode::setPublishPoint(std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { -// enablePointCloudPublishing_ = request.data; -// response.success = true; -// return true; -// } -// void ElevationMappingNode::updateVariance(const ros::TimerEvent&) { -// map_.update_variance(); -// } -// void ElevationMappingNode::updateTime(const ros::TimerEvent&) { -// map_.update_time(); -// } +void ElevationMappingNode::setPublishPoint(const std::shared_ptr request, + std::shared_ptr response) { + enablePointCloudPublishing_ = request->data; + response->success = true; +} -// void ElevationMappingNode::publishStatistics(const ros::TimerEvent&) { -// ros::Time now = ros::Time::now(); -// double dt = (now - lastStatisticsPublishedTime_).toSec(); -// lastStatisticsPublishedTime_ = now; -// elevation_map_msgs::Statistics msg; -// msg.header.stamp = now; -// if (dt > 0.0) { -// msg.pointcloud_process_fps = pointCloudProcessCounter_ / dt; -// } -// pointCloudProcessCounter_ = 0; -// statisticsPub_.publish(msg); -// } -// void ElevationMappingNode::updateGridMap(const ros::TimerEvent&) { -// std::vector layers(map_layers_all_.begin(), map_layers_all_.end()); -// std::lock_guard lock(mapMutex_); -// map_.get_grid_map(gridMap_, layers); -// gridMap_.setTimestamp(ros::Time::now().toNSec()); -// alivePub_.publish(std_msgs::Empty()); +void ElevationMappingNode::updateVariance() { + map_->update_variance(); +} -// // Mostly debug purpose -// if (enablePointCloudPublishing_) { -// publishAsPointCloud(gridMap_); -// } -// if (enableNormalArrowPublishing_) { -// publishNormalAsArrow(gridMap_); -// } -// isGridmapUpdated_ = true; -// } +void ElevationMappingNode::updateTime() { + map_->update_time(); +} + +void ElevationMappingNode::publishStatistics() { + auto now = this->now(); + double dt = (now - lastStatisticsPublishedTime_).seconds(); + lastStatisticsPublishedTime_ = now; + elevation_map_msgs::msg::Statistics msg; + msg.header.stamp = now; + if (dt > 0.0) { + msg.pointcloud_process_fps = pointCloudProcessCounter_ / dt; + } + pointCloudProcessCounter_ = 0; + statisticsPub_->publish(msg); +} + +void ElevationMappingNode::updateGridMap() { + std::vector layers(map_layers_all_.begin(), map_layers_all_.end()); + std::lock_guard lock(mapMutex_); + map_->get_grid_map(gridMap_, layers); + gridMap_.setTimestamp(this->now().nanoseconds()); + alivePub_->publish(std_msgs::msg::Empty()); + + // Mostly debug purpose + if (enablePointCloudPublishing_) { + publishAsPointCloud(gridMap_); + } + if (enableNormalArrowPublishing_) { + publishNormalAsArrow(gridMap_); + } + isGridmapUpdated_ = true; +} void ElevationMappingNode::initializeMap(const std::shared_ptr request, std::shared_ptr response) { @@ -813,65 +827,68 @@ void ElevationMappingNode::initializeMap(const std::shared_ptrsuccess = true; } -// void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const { -// auto startTime = ros::Time::now(); +void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) const { + auto startTime = this->now(); -// const auto& normalX = map["normal_x"]; -// const auto& normalY = map["normal_y"]; -// const auto& normalZ = map["normal_z"]; -// double scale = 0.1; + const auto& normalX = map["normal_x"]; + const auto& normalY = map["normal_y"]; + const auto& normalZ = map["normal_z"]; + double scale = 0.1; -// visualization_msgs::MarkerArray markerArray; -// // For each cell in map. -// for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { -// if (!map.isValid(*iterator, "elevation")) { -// continue; -// } -// grid_map::Position3 p; -// map.getPosition3("elevation", *iterator, p); -// Eigen::Vector3d start = p; -// const auto i = iterator.getLinearIndex(); -// Eigen::Vector3d normal(normalX(i), normalY(i), normalZ(i)); -// Eigen::Vector3d end = start + normal * scale; -// if (normal.norm() < 0.1) { -// continue; -// } -// markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); -// } -// normalPub_.publish(markerArray); -// ROS_INFO_THROTTLE(1.0, "publish as normal in %f sec.", (ros::Time::now() - startTime).toSec()); -// } + visualization_msgs::msg::MarkerArray markerArray; + // For each cell in map. + for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) { + if (!map.isValid(*iterator, "elevation")) { + continue; + } + grid_map::Position3 p; + map.getPosition3("elevation", *iterator, p); + Eigen::Vector3d start = p; + const auto i = iterator.getLinearIndex(); + Eigen::Vector3d normal(normalX(i), normalY(i), normalZ(i)); + Eigen::Vector3d end = start + normal * scale; + if (normal.norm() < 0.1) { + continue; + } + markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); + } + normalPub_->publish(markerArray); + double elapsed_time = (this->now() - startTime).seconds(); + RCLCPP_INFO(this->get_logger(), "Initializing ElevationMappingNode...");(this->get_logger(), std::chrono::seconds(1), "publish as normal in %f sec.", elapsed_time); +} -// visualization_msgs::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, -// const int id) const { -// visualization_msgs::Marker marker; -// marker.header.frame_id = mapFrameId_; -// marker.header.stamp = ros::Time::now(); -// marker.ns = "normal"; -// marker.id = id; -// marker.type = visualization_msgs::Marker::ARROW; -// marker.action = visualization_msgs::Marker::ADD; -// marker.points.resize(2); -// marker.points[0].x = start.x(); -// marker.points[0].y = start.y(); -// marker.points[0].z = start.z(); -// marker.points[1].x = end.x(); -// marker.points[1].y = end.y(); -// marker.points[1].z = end.z(); -// marker.pose.orientation.x = 0.0; -// marker.pose.orientation.y = 0.0; -// marker.pose.orientation.z = 0.0; -// marker.pose.orientation.w = 1.0; -// marker.scale.x = 0.01; -// marker.scale.y = 0.01; -// marker.scale.z = 0.01; -// marker.color.a = 1.0; // Don't forget to set the alpha! -// marker.color.r = 0.0; -// marker.color.g = 1.0; -// marker.color.b = 0.0; - -// return marker; -// } + + +visualization_msgs::msg::Marker ElevationMappingNode::vectorToArrowMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, + const int id) const { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = mapFrameId_; + marker.header.stamp = this->now(); + marker.ns = "normal"; + marker.id = id; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.points.resize(2); + marker.points[0].x = start.x(); + marker.points[0].y = start.y(); + marker.points[0].z = start.z(); + marker.points[1].x = end.x(); + marker.points[1].y = end.y(); + marker.points[1].z = end.z(); + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.01; + marker.scale.y = 0.01; + marker.scale.z = 0.01; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + + return marker; +} void ElevationMappingNode::publishMapToOdom(double error) { From 0526be49220890e8bba585b1040896187d35a592 Mon Sep 17 00:00:00 2001 From: amilearning Date: Fri, 8 Nov 2024 05:24:15 +0100 Subject: [PATCH 07/14] recording pointcloud update --- elevation_mapping_cupy/config/core/core_param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index c128291f..8b5bf32c 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -50,7 +50,7 @@ elevation_mapping: overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) pose_topic: '/odom' - map_frame: 'base_footprint' # The map frame where the odometry source uses. + map_frame: 'odom' # The map frame where the odometry source uses. base_frame: 'base_link' # The robot's base frame. This frame will be a center of the map. corrected_map_frame: 'odom' From b0d06c3e04862f355a921cedb28fcf62ebc12a5e Mon Sep 17 00:00:00 2001 From: HO JIN LEE Date: Fri, 8 Nov 2024 06:08:24 +0100 Subject: [PATCH 08/14] Update README.md --- README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/README.md b/README.md index 057424df..f2f8338d 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,16 @@ +## ROS2 Elevation Mapping Cupy + **Status**: Under Development 🚧 +### Features +- **Point cloud-based map update**: *Functional* +- **Image-based map update**: *Ongoing development* + +### Dependencies - +- **ROS 2 Humble** +- **CUDA 12.4** +- **PyTorch 2.4.0** + +![Elevation Map in ROS 2 Humble with Gazebo ](https://github.com/user-attachments/assets/0dd9ebbe-a90d-486f-9871-81921308fab9) + # Elevation Mapping cupy ![python tests](https://github.com/leggedrobotics/elevation_mapping_cupy/actions/workflows/python-tests.yml/badge.svg) From 80c2990c89d666bceb1036524f036fb456924c67 Mon Sep 17 00:00:00 2001 From: amilearning Date: Mon, 11 Nov 2024 06:06:42 +0900 Subject: [PATCH 09/14] orin topics --- .../config/core/core_param copy.yaml | 189 ++++++++++++++++++ .../config/core/core_param.yaml | 36 ++-- .../elevation_mapping_ros.hpp | 2 +- .../src/elevation_mapping_ros.cpp | 60 +++--- 4 files changed, 240 insertions(+), 47 deletions(-) create mode 100644 elevation_mapping_cupy/config/core/core_param copy.yaml diff --git a/elevation_mapping_cupy/config/core/core_param copy.yaml b/elevation_mapping_cupy/config/core/core_param copy.yaml new file mode 100644 index 00000000..bf5f1cd4 --- /dev/null +++ b/elevation_mapping_cupy/config/core/core_param copy.yaml @@ -0,0 +1,189 @@ +elevation_mapping: + ros__parameters: + #### Basic parameters ######## + enable_normal_arrow_publishing: true + cell_n: 40000 # number of cells in the map. + data_type: 'float32' # data type for the map. + average_weight: 0.5 + drift_compensation_variance_inlier: 0.1 + checker_layer: 'traversability' # layer name for checking the validity of the cell. + min_filter_size: 5 # size of the filter for min filter. + min_filter_iteration: 3 # number of iterations for min filter. + initialized_variance: 10.0 # initial variance for each cell. + resolution: 0.1 # resolution in m. + map_length: 20.0 # map's size in m. + sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). + mahalanobis_thresh: 2.0 # points outside this distance is outlier. + outlier_variance: 0.01 # if point is outlier, add this value to the cell. + drift_compensation_variance_inler: 0.05 # cells under this value is used for drift compensation. + max_drift: 0.1 # drift compensation happens only the drift is smaller than this value (for safety) + drift_compensation_alpha: 0.1 # drift compensation alpha for smoother update of drift compensation + time_variance: 0.0001 # add this value when update_variance is called. + max_variance: 100.0 # maximum variance for each cell. + initial_variance: 1000.0 # initial variance for each cell. + traversability_inlier: 0.9 # cells with higher traversability are used for drift compensation. + dilation_size: 3 # dilation filter size before traversability filter. + wall_num_thresh: 20 # if there are more points than this value, only higher points than the current height are used to make the wall more sharp. + min_height_drift_cnt: 100 # drift compensation only happens if the valid cells are more than this number. + position_noise_thresh: 0.01 # if the position change is bigger than this value, the drift compensation happens. + orientation_noise_thresh: 0.01 # if the orientation change is bigger than this value, the drift compensation happens. + position_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. + orientation_lowpass_alpha: 0.2 # lowpass filter alpha used for detecting movements. + min_valid_distance: 0.5 # points with shorter distance will be filtered out. + max_height_range: 1.0 # points higher than this value from sensor will be filtered out to disable ceiling. + ramped_height_range_a: 0.3 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + ramped_height_range_b: 1.0 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + ramped_height_range_c: 0.2 # if z > max(d - ramped_height_range_b, 0) * ramped_height_range_a + ramped_height_range_c, reject. + update_variance_fps: 5.0 # fps for updating variance. + update_pose_fps: 10.0 # fps for updating pose and shift the center of map. + time_interval: 0.1 # Time layer is updated with this interval. + map_acquire_fps: 5.0 # Raw map is fetched from GPU memory in this fps. + publish_statistics_fps: 1.0 # Publish statistics topic in this fps. + + max_ray_length: 10.0 # maximum length for ray tracing. + cleanup_step: 0.1 # subtitute this value from validity layer at visibiltiy cleanup. + cleanup_cos_thresh: 0.1 # subtitute this value from validity layer at visibiltiy cleanup. + + safe_thresh: 0.7 # if traversability is smaller, it is counted as unsafe cell. + safe_min_thresh: 0.4 # polygon is unsafe if there exists lower traversability than this. + max_unsafe_n: 10 # if the number of cells under safe_thresh exceeds this value, polygon is unsafe. + + overlap_clear_range_xy: 4.0 # xy range [m] for clearing overlapped area. this defines the valid area for overlap clearance. (used for multi floor setting) + overlap_clear_range_z: 2.0 # z range [m] for clearing overlapped area. cells outside this range will be cleared. (used for multi floor setting) + pose_topic: '/odom' + map_frame: 'odom' # The map frame where the odometry source uses. + base_frame: 'base_link' # The robot's base frame. This frame will be a center of the map. + corrected_map_frame: 'odom' + + + + #### Feature toggles ######## + enable_edge_sharpen: true + enable_visibility_cleanup: true + enable_drift_compensation: true + enable_overlap_clearance: true + enable_pointcloud_publishing: false + enable_drift_corrected_TF_publishing: false + enable_normal_color: true # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. + + + #### Traversability filter ######## + use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. + weight_file: '/share/elevation_mapping_cupy/config/core/weights.dat' # Weight file for traversability filter + plugin_config_file: '/share/elevation_mapping_cupy/config/core/plugin_config.yaml' # Configuration file for the plugin. + #### Upper bound ######## + use_only_above_for_upper_bound: false + + #### Initializer ######## + initialize_method: 'linear' # Choose one from 'nearest', 'linear', 'cubic' + initialize_frame_id: ['base_footprint'] # One tf (like ['footprint'] ) initializes a square around it. + initialize_tf_offset: [0.0, 0.0, 0.0, 0.0] # z direction. Should be same number as initialize_frame_id. + dilation_size_initialize: 2 # dilation size after the init. + initialize_tf_grid_size: 0.5 # This is not used if number of tf is more than 3. + use_initializer_at_start: true # Use initializer when the node starts. + + #### Default Plugins ######## + + pointcloud_channel_fusions: + rgb: 'color' # 'color' fusion is used for the 'rgb' channel + default: 'average' # 'average' fusion is used for channels not listed here + + image_channel_fusions: + rgb: 'color' # 'color' fusion is used for the 'rgb' channel + default: 'exponential' # 'exponential' fusion is used for channels not listed here + feat_.*: 'exponential' # 'exponential' fusion is also used for any channel starting with 'feat_' Regular expressions can be used for channel names + + #### Subscribers ######## + # pointcloud_sensor_name: + # topic_name: '/sensor/pointcloud_semantic' + # data_type: pointcloud # pointcloud or image + # + # image_sensor_name: + # topic_name: '/camera/image_semantic' + # data_type: image # pointcloud or image + # camera_info_topic_name: '/camera/depth/camera_info' + # channel_info_topic_name: '/camera/channel_info' + + + subscribers: + pointcloud1: + # topic_name: '/ouster/points' + topic_name: '/a200/sensors/camera_0/points' + data_type: 'pointcloud' + image1: + topic_name: '/a200/sensors/camera_0/color/image' + info_name: '/a200/sensors/camera_0/color/camera_info' + data_type: 'image' + # channel_name: '/front_cam/channel_info' + + # image_topic2: '/camera/rgb/image_raw2' + # image_info2: '/camera/depth/camera_info2' + # image_channel_info1: '/front_cam/channel_info' + + # image: # for semantic images + # topic_name: '/front_cam/semantic_image' + # camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized' + # channel_info_topic_name: '/front_cam/channel_info' + # data_type: image + + #### Publishers ######## + # topic_name: + # layers: # Choose from 'elevation', 'variance', 'traversability', 'time', 'normal_x', 'normal_y', 'normal_z', 'color', plugin_layer_names + # basic_layers: # basic_layers for valid cell computation (e.g. Rviz): Choose a subset of `layers`. + # fps: # Publish rate. Use smaller value than `map_acquire_fps`. + + publishers: + elevation_map_raw: + layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z'] + basic_layers: ['elevation'] + fps: 5.0 + elevation_map_recordable: + layers: ['elevation', 'traversability'] + basic_layers: ['elevation', 'traversability'] + fps: 2.0 + elevation_map_filter: + layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] + basic_layers: ['min_filter'] + fps: 3.0 + + + # plugun info + + min_filter: + enable: True # whether to load this plugin + fill_nan: False # Fill nans to invalid cells of elevation layer. + is_height_layer: True # If this is a height layer (such as elevation) or not (such as traversability) + layer_name: "min_filter" # The layer name. + extra_params: # These params are passed to the plugin class on initialization. + dilation_size: 1 # The patch size to apply + iteration_n: 30 # The number of iterations + + # Apply smoothing. + smooth_filter: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "smooth" + extra_params: + input_layer_name: "min_filter" + + # Apply inpainting using opencv + inpainting: + enable: True + fill_nan: False + is_height_layer: True + layer_name: "inpaint" + extra_params: + method: "telea" # telea or ns + + # Apply smoothing for inpainted layer + erosion: + enable: True + fill_nan: False + is_height_layer: False + layer_name: "erosion" + extra_params: + input_layer_name: "traversability" + dilation_size: 3 + iteration_n: 20 + reverse: True \ No newline at end of file diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 8b5bf32c..6facd7d9 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,6 +1,7 @@ elevation_mapping: ros__parameters: - #### Basic parameters ######## + #### Basic parameters ######## + enable_normal_arrow_publishing: false cell_n: 40000 # number of cells in the map. data_type: 'float32' # data type for the map. average_weight: 0.5 @@ -10,7 +11,7 @@ elevation_mapping: min_filter_iteration: 3 # number of iterations for min filter. initialized_variance: 10.0 # initial variance for each cell. resolution: 0.1 # resolution in m. - map_length: 20.0 # map's size in m. + map_length: 20.0 # map's size in m. sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). mahalanobis_thresh: 2.0 # points outside this distance is outlier. outlier_variance: 0.01 # if point is outlier, add this value to the cell. @@ -63,8 +64,8 @@ elevation_mapping: enable_overlap_clearance: true enable_pointcloud_publishing: false enable_drift_corrected_TF_publishing: false - enable_normal_color: false # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. - enable_normal: false + enable_normal_color: true # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. + enable_normal: true #### Traversability filter ######## use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. @@ -105,12 +106,13 @@ elevation_mapping: subscribers: pointcloud1: + # topic_name: '/ouster/points' topic_name: '/a200/sensors/camera_0/points' data_type: 'pointcloud' - image1: - topic_name: '/a200/sensors/camera_0/color/image' - info_name: '/a200/sensors/camera_0/color/camera_info' - data_type: 'image' + # image1: + # topic_name: '/a200/sensors/camera_0/color/image' + # info_name: '/a200/sensors/camera_0/color/camera_info' + # data_type: 'image' # channel_name: '/front_cam/channel_info' # image_topic2: '/camera/rgb/image_raw2' @@ -133,17 +135,17 @@ elevation_mapping: publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance','rgb'] + layers: ['elevation', 'traversability', 'variance','normal_x', 'normal_y', 'normal_z'] basic_layers: ['elevation'] fps: 5.0 - elevation_map_recordable: - layers: ['elevation', 'traversability'] - basic_layers: ['elevation', 'traversability'] - fps: 2.0 - elevation_map_filter: - layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] - basic_layers: ['min_filter'] - fps: 3.0 + # elevation_map_recordable: + # layers: ['elevation', 'traversability'] + # basic_layers: ['elevation', 'traversability'] + # fps: 2.0 + # elevation_map_filter: + # layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] + # basic_layers: ['min_filter'] + # fps: 3.0 # plugun info diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index 7cf91ce1..220509e4 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -14,7 +14,7 @@ // Pybind #include // everything needed for embedding - +#include // ROS #include #include diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 57f8bd83..8825b97e 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -132,29 +132,29 @@ ElevationMappingNode::ElevationMappingNode() // } else { // transport_hint = "raw"; // In the default case we assume raw topic // } - - ImageSubscriberPtr image_sub = std::make_shared(this, camera_topic, rmw_qos_profile_sensor_data); - imageSubs_.push_back(image_sub); - - CameraInfoSubscriberPtr cam_info_sub = std::make_shared(this, info_topic, rmw_qos_profile_sensor_data); - cameraInfoSubs_.push_back(cam_info_sub); - - std::string channel_info_topic; - if (this->get_parameter("subscribers." + sub_name + ".channel_name", channel_info_topic)) { - ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(this, channel_info_topic, rmw_qos_profile_sensor_data); - channelInfoSubs_.push_back(channel_info_sub); - CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); - sync->registerCallback(std::bind(&ElevationMappingNode::imageChannelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cameraChannelSyncs_.push_back(sync); - RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s, Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), channel_info_topic.c_str()); - }else{ - std::string key = sub_name; - channels_[key].push_back("rgb"); - // RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s. Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")).c_str() : channel_info_topic.c_str())); - CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); - sync->registerCallback(std::bind(&ElevationMappingNode::imageCallback, this, std::placeholders::_1, std::placeholders::_2, key)); - cameraSyncs_.push_back(sync); - } + + ImageSubscriberPtr image_sub = std::make_shared(this, camera_topic, rmw_qos_profile_sensor_data); + imageSubs_.push_back(image_sub); + + CameraInfoSubscriberPtr cam_info_sub = std::make_shared(this, info_topic, rmw_qos_profile_sensor_data); + cameraInfoSubs_.push_back(cam_info_sub); + + std::string channel_info_topic; + if (this->get_parameter("subscribers." + sub_name + ".channel_name", channel_info_topic)) { + ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(this, channel_info_topic, rmw_qos_profile_sensor_data); + channelInfoSubs_.push_back(channel_info_sub); + CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); + sync->registerCallback(std::bind(&ElevationMappingNode::imageChannelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + cameraChannelSyncs_.push_back(sync); + RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s, Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), channel_info_topic.c_str()); + }else{ + std::string key = sub_name; + channels_[key].push_back("rgb"); + // RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s. Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")).c_str() : channel_info_topic.c_str())); + CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); + sync->registerCallback(std::bind(&ElevationMappingNode::imageCallback, this, std::placeholders::_1, std::placeholders::_2, key)); + cameraSyncs_.push_back(sync); + } }else if(data_type == "pointcloud"){ std::string pointcloud_topic; this->get_parameter("subscribers." + sub_name + ".topic_name", pointcloud_topic); @@ -162,10 +162,13 @@ ElevationMappingNode::ElevationMappingNode() channels_[key].push_back("x"); channels_[key].push_back("y"); channels_[key].push_back("z"); + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, qos_profile.depth), qos_profile); + auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { this->pointcloudCallback(msg, key); }; - auto sub = this->create_subscription(pointcloud_topic, 1, callback); + auto sub = this->create_subscription(pointcloud_topic, qos, callback); pointcloudSubs_.push_back(sub); RCLCPP_INFO(this->get_logger(), "Subscribed to PointCloud2 topic: %s", pointcloud_topic.c_str()); } @@ -356,8 +359,8 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud std::vector channels; for (size_t it = 0; it < fields.size(); it++) { - auto& field = fields[it]; - channels.push_back(field.name); + auto& field = fields[it]; + channels.push_back(field.name); } inputPointCloud(cloud, channels); @@ -775,6 +778,7 @@ void ElevationMappingNode::updateGridMap() { if (enablePointCloudPublishing_) { publishAsPointCloud(gridMap_); } + if (enableNormalArrowPublishing_) { publishNormalAsArrow(gridMap_); } @@ -852,9 +856,7 @@ void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) co } markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); } - normalPub_->publish(markerArray); - double elapsed_time = (this->now() - startTime).seconds(); - RCLCPP_INFO(this->get_logger(), "Initializing ElevationMappingNode...");(this->get_logger(), std::chrono::seconds(1), "publish as normal in %f sec.", elapsed_time); + normalPub_->publish(markerArray); } From 4f35ca8a50bb8adc5f90d203fc60d123b237dbce Mon Sep 17 00:00:00 2001 From: amilearning Date: Thu, 5 Dec 2024 14:01:59 +0100 Subject: [PATCH 10/14] image subscribe --- .../config/core/core_param.yaml | 43 +-- .../elevation_mapping_ros.hpp | 54 ++-- .../src/elevation_mapping_ros.cpp | 286 +++++++++++------- .../src/elevation_mapping_wrapper.cpp | 2 +- 4 files changed, 235 insertions(+), 150 deletions(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 6facd7d9..3a693641 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,7 +1,8 @@ elevation_mapping: ros__parameters: - #### Basic parameters ######## - enable_normal_arrow_publishing: false + #### Basic parameters ######## + voxel_filter_size: 0.05 + enable_normal_arrow_publishing: true cell_n: 40000 # number of cells in the map. data_type: 'float32' # data type for the map. average_weight: 0.5 @@ -11,7 +12,7 @@ elevation_mapping: min_filter_iteration: 3 # number of iterations for min filter. initialized_variance: 10.0 # initial variance for each cell. resolution: 0.1 # resolution in m. - map_length: 20.0 # map's size in m. + map_length: 20.0 # map's size in m. sensor_noise_factor: 0.05 # point's noise is sensor_noise_factor*z^2 (z is distance from sensor). mahalanobis_thresh: 2.0 # points outside this distance is outlier. outlier_variance: 0.01 # if point is outlier, add this value to the cell. @@ -65,7 +66,7 @@ elevation_mapping: enable_pointcloud_publishing: false enable_drift_corrected_TF_publishing: false enable_normal_color: true # If true, the map contains 'color' layer corresponding to normal. Add 'color' layer to the publishers setting if you want to visualize. - enable_normal: true + #### Traversability filter ######## use_chainer: false # Use chainer as a backend of traversability filter or pytorch. If false, it uses pytorch. pytorch requires ~2GB more GPU memory compared to chainer but runs faster. @@ -104,23 +105,29 @@ elevation_mapping: # camera_info_topic_name: '/camera/depth/camera_info' # channel_info_topic_name: '/camera/channel_info' + subscribers: pointcloud1: - # topic_name: '/ouster/points' - topic_name: '/a200/sensors/camera_0/points' + # topic_name: '/ouster/points' + topic_name: '/a200/sensors/camera_0/points' data_type: 'pointcloud' - # image1: + image1: + topic_name: '/feat_processing_node/semantic_seg_feat' + info_name: '/feat_processing_node/a200/sensors/camera_0/color/camera_info_resized' + channel_name: '/feat_processing_node/feat_channel_info' + data_type: 'image' + # image2: # topic_name: '/a200/sensors/camera_0/color/image' # info_name: '/a200/sensors/camera_0/color/camera_info' # data_type: 'image' # channel_name: '/front_cam/channel_info' + # channel_name: '/front_cam/channel_info' + # image_topic2: '/camera/rgb/image_raw2' # image_info2: '/camera/depth/camera_info2' # image_channel_info1: '/front_cam/channel_info' - - # image: # for semantic images # topic_name: '/front_cam/semantic_image' # camera_info_topic_name: '/front_cam/camera/depth/camera_info_resized' @@ -135,17 +142,17 @@ elevation_mapping: publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance','normal_x', 'normal_y', 'normal_z'] + layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2','feat_3','feat_4'] basic_layers: ['elevation'] fps: 5.0 - # elevation_map_recordable: - # layers: ['elevation', 'traversability'] - # basic_layers: ['elevation', 'traversability'] - # fps: 2.0 - # elevation_map_filter: - # layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] - # basic_layers: ['min_filter'] - # fps: 3.0 + elevation_map_recordable: + layers: ['elevation', 'traversability'] + basic_layers: ['elevation', 'traversability'] + fps: 2.0 + elevation_map_filter: + layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] + basic_layers: ['min_filter'] + fps: 3.0 # plugun info diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index 220509e4..aa843e8f 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -48,6 +48,7 @@ #include #include #include +#include // OpenCV #include @@ -89,23 +90,28 @@ class ElevationMappingNode : public rclcpp::Node { using CameraChannelSyncPtr = std::shared_ptr; // Subscriber and Synchronizer for Pointcloud messages - using PointCloudSubscriber = message_filters::Subscriber; - using PointCloudSubscriberPtr = std::shared_ptr; - using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; - using PointCloudSync = message_filters::Synchronizer; - using PointCloudSyncPtr = std::shared_ptr; + // using PointCloudSubscriber = message_filters::Subscriber; + // using PointCloudSubscriberPtr = std::shared_ptr; + // using PointCloudPolicy = message_filters::sync_policies::ApproximateTime; + // using PointCloudSync = message_filters::Synchronizer; + // using PointCloudSyncPtr = std::shared_ptr; private: - void readParameters(); - void setupMapPublishers(); - void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key); - void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector& channels); -// void inputImage(const sensor_msgs::ImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg, const std::vector& channels); +void readParameters(); +void setupMapPublishers(); +void pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key); +void inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector& channels); + +void inputImage(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg, + const std::vector& channels); + + // void imageCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const std::string& key); // void imageChannelCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const sensor_msgs::msg::CameraInfo::SharedPtr camera_info_msg, const elevation_map_msgs::msg::ChannelInfo::SharedPtr channel_info_msg); -void imageCallback(const std::shared_ptr& image_msg, const std::shared_ptr& camera_info_msg, const std::string& key); -void imageChannelCallback(const std::shared_ptr& image_msg, const std::shared_ptr& camera_info_msg, const std::shared_ptr& channel_info_msg); - +void imageCallback(const sensor_msgs::msg::Image::SharedPtr cloud, const std::string& key); +void imageChannelCallback(const elevation_map_msgs::msg::ChannelInfo::SharedPtr chennel_info, const std::string& key); +void imageInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr image_info, const std::string& key); // void pointCloudChannelCallback(const sensor_msgs::PointCloud2& cloud, const elevation_map_msgs::ChannelInfoConstPtr& channel_info_msg); // // void multiLayerImageCallback(const elevation_map_msgs::MultiLayerImageConstPtr& image_msg, const sensor_msgs::CameraInfoConstPtr& camera_info_msg); void publishAsPointCloud(const grid_map::GridMap& map) const; @@ -139,12 +145,18 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start rclcpp::Node::SharedPtr node_; // image_transport::ImageTransport it_; std::vector::SharedPtr> pointcloudSubs_; - std::vector imageSubs_; - std::vector cameraInfoSubs_; - std::vector channelInfoSubs_; - std::vector cameraSyncs_; - std::vector cameraChannelSyncs_; - std::vector pointCloudSyncs_; + + std::vector::SharedPtr> imageSubs_; + std::vector::SharedPtr> cameraInfoSubs_; + std::vector::SharedPtr> channelInfoSubs_; + + + // std::vector imageSubs_; + // std::vector cameraInfoSubs_; + // std::vector channelInfoSubs_; + // std::vector cameraSyncs_; + // std::vector cameraChannelSyncs_; + // std::vector pointCloudSyncs_; std::vector::SharedPtr> mapPubs_; @@ -206,6 +218,7 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start double positionAlpha_; double orientationAlpha_; + double voxel_filter_size_; double recordableFps_; std::atomic_bool enablePointCloudPublishing_; @@ -215,6 +228,9 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start double initializeTfGridSize_; bool alwaysClearWithInitializer_; std::atomic_int pointCloudProcessCounter_; + + std::map> imageInfoReady_; + std::map> imageChannelReady_; }; } // namespace elevation_mapping_cupy diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 8825b97e..1e3f5505 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -70,6 +70,7 @@ ElevationMappingNode::ElevationMappingNode() this->get_parameter("enable_drift_corrected_TF_publishing", enableDriftCorrectedTFPublishing_); this->get_parameter("use_initializer_at_start", useInitializerAtStart_); this->get_parameter("always_clear_with_initializer", alwaysClearWithInitializer_); + this->get_parameter("voxel_filter_size", voxel_filter_size_); RCLCPP_INFO(this->get_logger(), "initialize_frame_id: %s", initialize_frame_id_.empty() ? "[]" : initialize_frame_id_[0].c_str()); RCLCPP_INFO(this->get_logger(), "initialize_tf_offset: [%f, %f, %f, %f]", initialize_tf_offset_[0], initialize_tf_offset_[1], initialize_tf_offset_[2], initialize_tf_offset_[3]); @@ -132,39 +133,59 @@ ElevationMappingNode::ElevationMappingNode() // } else { // transport_hint = "raw"; // In the default case we assume raw topic // } + + std::string key = sub_name; + - ImageSubscriberPtr image_sub = std::make_shared(this, camera_topic, rmw_qos_profile_sensor_data); - imageSubs_.push_back(image_sub); - - CameraInfoSubscriberPtr cam_info_sub = std::make_shared(this, info_topic, rmw_qos_profile_sensor_data); - cameraInfoSubs_.push_back(cam_info_sub); - - std::string channel_info_topic; - if (this->get_parameter("subscribers." + sub_name + ".channel_name", channel_info_topic)) { - ChannelInfoSubscriberPtr channel_info_sub = std::make_shared(this, channel_info_topic, rmw_qos_profile_sensor_data); - channelInfoSubs_.push_back(channel_info_sub); - CameraChannelSyncPtr sync = std::make_shared(CameraChannelPolicy(10), *image_sub, *cam_info_sub, *channel_info_sub); - sync->registerCallback(std::bind(&ElevationMappingNode::imageChannelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cameraChannelSyncs_.push_back(sync); - RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s, Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), channel_info_topic.c_str()); - }else{ - std::string key = sub_name; - channels_[key].push_back("rgb"); - // RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s, Camera info topic: %s. Channel info topic: %s", camera_topic.c_str(), info_topic.c_str(), (channel_info_topic.empty() ? ("Not found. Using channels: " + boost::algorithm::join(channels_[key], ", ")).c_str() : channel_info_topic.c_str())); - CameraSyncPtr sync = std::make_shared(CameraPolicy(10), *image_sub, *cam_info_sub); - sync->registerCallback(std::bind(&ElevationMappingNode::imageCallback, this, std::placeholders::_1, std::placeholders::_2, key)); - cameraSyncs_.push_back(sync); - } - }else if(data_type == "pointcloud"){ + sensor_msgs::msg::CameraInfo img_info; + elevation_map_msgs::msg::ChannelInfo channel_info; + imageInfoReady_[key] = std::make_pair(img_info, false); + imageChannelReady_[key] = std::make_pair(channel_info, false); + // Image subscriber init + + rmw_qos_profile_t sensor_qos_profile = rmw_qos_profile_sensor_data; + auto sensor_qos = rclcpp::QoS(rclcpp::QoSInitialization(sensor_qos_profile.history, sensor_qos_profile.depth), sensor_qos_profile); + + auto img_callback = [this, key](const sensor_msgs::msg::Image::SharedPtr msg) { + this->imageCallback(msg, key); + }; + auto img_sub = this->create_subscription(camera_topic, sensor_qos, img_callback); + imageSubs_.push_back(img_sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to Image topic: %s", camera_topic.c_str()); + // Camera Info subscriber init + auto img_info_callback = [this, key](const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + this->imageInfoCallback(msg, key); + }; + auto img_info_sub = this->create_subscription(info_topic, sensor_qos, img_info_callback); + cameraInfoSubs_.push_back(img_info_sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to ImageInfo topic: %s", info_topic.c_str()); + + std::string channel_info_topic; + if (this->get_parameter("subscribers." + sub_name + ".channel_name", channel_info_topic)) { + // channel subscriber init + imageChannelReady_[key].second = false; + auto img_channel_callback = [this, key](const elevation_map_msgs::msg::ChannelInfo::SharedPtr msg) { + this->imageChannelCallback(msg, key); + }; + auto channel_info_sub = this->create_subscription(channel_info_topic, sensor_qos, img_channel_callback); + channelInfoSubs_.push_back(channel_info_sub); + RCLCPP_INFO(this->get_logger(), "Subscribed to ChannelInfo topic: %s", channel_info_topic.c_str()); + }else{ + imageChannelReady_[key].second = true; + channels_[key].push_back("rgb"); + } + }else if(data_type == "pointcloud"){ std::string pointcloud_topic; this->get_parameter("subscribers." + sub_name + ".topic_name", pointcloud_topic); std::string key = sub_name; channels_[key].push_back("x"); channels_[key].push_back("y"); channels_[key].push_back("z"); + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, qos_profile.depth), qos_profile); + auto callback = [this, key](const sensor_msgs::msg::PointCloud2::SharedPtr msg) { this->pointcloudCallback(msg, key); }; @@ -352,6 +373,29 @@ void ElevationMappingNode::publishMapOfIndex(int index) { mapPubs_[index]->publish(msg); } +void ElevationMappingNode::imageInfoCallback(const sensor_msgs::msg::CameraInfo::SharedPtr image_info, const std::string& key) { +imageInfoReady_[key] = std::make_pair(*image_info, true); + // Find and remove the subscription for this key + auto it = std::find_if(cameraInfoSubs_.begin(), cameraInfoSubs_.end(), + [key](const rclcpp::Subscription::SharedPtr& sub) { + return sub->get_topic_name() == key; + }); + if (it != cameraInfoSubs_.end()) { + cameraInfoSubs_.erase(it); + } + +} + +void ElevationMappingNode::imageChannelCallback(const elevation_map_msgs::msg::ChannelInfo::SharedPtr channel_info, const std::string& key) { +imageChannelReady_[key] = std::make_pair(*channel_info, true); + auto it = std::find_if(channelInfoSubs_.begin(), channelInfoSubs_.end(), + [key](const rclcpp::Subscription::SharedPtr& sub) { + return sub->get_topic_name() == key; + }); + if (it != channelInfoSubs_.end()) { + channelInfoSubs_.erase(it); + } +} void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::string& key) { // get channels @@ -359,8 +403,8 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud std::vector channels; for (size_t it = 0; it < fields.size(); it++) { - auto& field = fields[it]; - channels.push_back(field.name); + auto& field = fields[it]; + channels.push_back(field.name); } inputPointCloud(cloud, channels); @@ -371,9 +415,19 @@ void ElevationMappingNode::pointcloudCallback(const sensor_msgs::msg::PointCloud void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2::SharedPtr cloud, const std::vector& channels) { auto start = this->now(); - auto* pcl_pc = new pcl::PCLPointCloud2; - pcl::PCLPointCloud2ConstPtr cloudPtr(pcl_pc); - pcl_conversions::toPCL(*cloud, *pcl_pc); + // auto* raw_pcl_pc = new pcl::PCLPointCloud2; + // pcl::PCLPointCloud2ConstPtr cloudPtr(raw_pcl_pc); + pcl::PCLPointCloud2::Ptr raw_pcl_pc(new pcl::PCLPointCloud2()); + pcl_conversions::toPCL(*cloud, *raw_pcl_pc); + + // apply the voxel filtering + pcl::PCLPointCloud2::Ptr pcl_pc (new pcl::PCLPointCloud2()); + pcl::VoxelGrid voxel_filter; + voxel_filter.setInputCloud(raw_pcl_pc); + voxel_filter.setLeafSize(voxel_filter_size_,voxel_filter_size_,voxel_filter_size_); + voxel_filter.filter(*pcl_pc); + + RCLCPP_DEBUG(this->get_logger(), "Voxel grid filtered point cloud from %d points to %d points.", static_cast(raw_pcl_pc->width * raw_pcl_pc->height), static_cast(pcl_pc->width * pcl_pc->height)); // Get channels auto fields = cloud->fields; @@ -427,98 +481,106 @@ void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2:: -// void ElevationMappingNode::inputImage(const sensor_msgs::ImageConstPtr& image_msg, -// const sensor_msgs::CameraInfoConstPtr& camera_info_msg, -// const std::vector& channels) { -// // Get image -// cv::Mat image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; +void ElevationMappingNode::inputImage(const sensor_msgs::msg::Image::ConstSharedPtr& image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& camera_info_msg, + const std::vector& channels) { + // Get image + cv::Mat image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; -// // Change encoding to RGB/RGBA -// if (image_msg->encoding == "bgr8") { -// cv::cvtColor(image, image, CV_BGR2RGB); -// } else if (image_msg->encoding == "bgra8") { -// cv::cvtColor(image, image, CV_BGRA2RGBA); -// } + // Change encoding to RGB/RGBA + if (image_msg->encoding == "bgr8") { + cv::cvtColor(image, image, CV_BGR2RGB); + } else if (image_msg->encoding == "bgra8") { + cv::cvtColor(image, image, CV_BGRA2RGBA); + } -// // Extract camera matrix -// Eigen::Map> cameraMatrix(&camera_info_msg->K[0]); - -// // Extract distortion coefficients -// Eigen::VectorXd distortionCoeffs; -// if (!camera_info_msg->D.empty()) { -// distortionCoeffs = Eigen::Map(camera_info_msg->D.data(), camera_info_msg->D.size()); -// } else { -// ROS_WARN("Distortion coefficients are empty."); -// distortionCoeffs = Eigen::VectorXd::Zero(5); -// // return; -// } + // Extract camera matrix + Eigen::Map> cameraMatrix(&camera_info_msg->k[0]); -// // distortion model -// std::string distortion_model = camera_info_msg->distortion_model; - -// // Get pose of sensor in map frame -// tf::StampedTransform transformTf; -// std::string sensorFrameId = image_msg->header.frame_id; -// auto timeStamp = image_msg->header.stamp; -// Eigen::Affine3d transformationMapToSensor; -// try { -// transformListener_.waitForTransform(sensorFrameId, mapFrameId_, timeStamp, ros::Duration(1.0)); -// transformListener_.lookupTransform(sensorFrameId, mapFrameId_, timeStamp, transformTf); -// poseTFToEigen(transformTf, transformationMapToSensor); -// } catch (tf::TransformException& ex) { -// ROS_ERROR("%s", ex.what()); -// return; -// } + // Extract distortion coefficients + Eigen::VectorXd distortionCoeffs; + if (!camera_info_msg->d.empty()) { + distortionCoeffs = Eigen::Map(camera_info_msg->d.data(), camera_info_msg->d.size()); + } else { + RCLCPP_WARN(this->get_logger(), "Distortion coefficients are empty."); + distortionCoeffs = Eigen::VectorXd::Zero(5); + // return; + } -// // Transform image to vector of Eigen matrices for easy pybind conversion -// std::vector image_split; -// std::vector multichannel_image; -// cv::split(image, image_split); -// for (auto img : image_split) { -// ColMatrixXf eigen_img; -// cv::cv2eigen(img, eigen_img); -// multichannel_image.push_back(eigen_img); -// } + // distortion model + std::string distortion_model = camera_info_msg->distortion_model; + + // Get pose of sensor in map frame + geometry_msgs::msg::TransformStamped transformStamped; + std::string sensorFrameId = image_msg->header.frame_id; + auto timeStamp = image_msg->header.stamp; + Eigen::Isometry3d transformationMapToSensor; + try { + transformStamped = tfBuffer_->lookupTransform(sensorFrameId, mapFrameId_, tf2::TimePointZero); + transformationMapToSensor = tf2::transformToEigen(transformStamped); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + return; + } -// // Check if the size of multichannel_image and channels and channel_methods matches. "rgb" counts for 3 layers. -// int total_channels = 0; -// for (const auto& channel : channels) { -// if (channel == "rgb") { -// total_channels += 3; -// } else { -// total_channels += 1; -// } -// } -// if (total_channels != multichannel_image.size()) { -// ROS_ERROR("Mismatch in the size of multichannel_image (%d), channels (%d). Please check the input.", multichannel_image.size(), channels.size()); -// ROS_ERROR_STREAM("Current Channels: " << boost::algorithm::join(channels, ", ")); -// return; -// } + // Transform image to vector of Eigen matrices for easy pybind conversion + std::vector image_split; + std::vector multichannel_image; + cv::split(image, image_split); + for (auto img : image_split) { + ColMatrixXf eigen_img; + cv::cv2eigen(img, eigen_img); + multichannel_image.push_back(eigen_img); + } -// // Pass image to pipeline -// map_.input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, -// distortionCoeffs, distortion_model, image.rows, image.cols); -// } + // Check if the size of multichannel_image and channels and channel_methods matches. "rgb" counts for 3 layers. + int total_channels = 0; + for (const auto& channel : channels) { + if (channel == "rgb") { + total_channels += 3; + } else { + total_channels += 1; + } + } + if (total_channels != multichannel_image.size()) { + RCLCPP_ERROR(this->get_logger(), "Mismatch in the size of multichannel_image (%d), channels (%d). Please check the input.", multichannel_image.size(), channels.size()); + for (const auto& channel : channels) { + RCLCPP_INFO(this->get_logger(), "Channel: %s", channel.c_str()); + } + return; + } -void ElevationMappingNode::imageCallback(const std::shared_ptr& image_msg, - const std::shared_ptr& camera_info_msg, - const std::string& key) { - auto start = this->now(); - // inputImage(image_msg, camera_info_msg, channels_[key]); - RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageCallback processed an image in %f sec.", (this->now() - start).seconds()); - + // Pass image to pipeline + map_->input_image(multichannel_image, channels, transformationMapToSensor.rotation(), transformationMapToSensor.translation(), cameraMatrix, + distortionCoeffs, distortion_model, image.rows, image.cols); } -void ElevationMappingNode::imageChannelCallback(const std::shared_ptr& image_msg, - const std::shared_ptr& camera_info_msg, - const std::shared_ptr& channel_info_msg) { -auto start = this->now(); -// Default channels and fusion methods for image is rgb and image_color -// std::vector channels; -// channels = channel_info_msg->channels; -// inputImage(image_msg, camera_info_msg, channels); -RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); + +void ElevationMappingNode::imageCallback(const sensor_msgs::msg::Image::SharedPtr image_msg, const std::string& key){ + auto start = this->now(); + + if (!imageInfoReady_[key].second){ + RCLCPP_WARN(this->get_logger(), "CameraInfo for key %s is not available yet.", key.c_str()); + return; + } + + + auto camera_info_msg = std::make_shared(imageInfoReady_[key].first); + if (std::find(channels_[key].begin(), channels_[key].end(), "rgb") != channels_[key].end()){ + inputImage(image_msg, camera_info_msg, channels_[key]); + } + else{ + if (!imageChannelReady_[key].second){ + RCLCPP_WARN(this->get_logger(), "ChannelInfo for key %s is not available yet.", key.c_str()); + return; + } + // Default channels and fusion methods for image is rgb and image_color + std::vector channels; + channels = imageChannelReady_[key].first.channels; + inputImage(image_msg, camera_info_msg, channels); + } + RCLCPP_INFO(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); } @@ -778,7 +840,6 @@ void ElevationMappingNode::updateGridMap() { if (enablePointCloudPublishing_) { publishAsPointCloud(gridMap_); } - if (enableNormalArrowPublishing_) { publishNormalAsArrow(gridMap_); } @@ -856,7 +917,8 @@ void ElevationMappingNode::publishNormalAsArrow(const grid_map::GridMap& map) co } markerArray.markers.push_back(vectorToArrowMarker(start, end, i)); } - normalPub_->publish(markerArray); + normalPub_->publish(markerArray); + } diff --git a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp index 0e9f29c5..a90fae93 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_wrapper.cpp @@ -205,7 +205,7 @@ void ElevationMappingWrapper::setParameters() { map_n_ = py::cast(param_.attr("get_value")("true_cell_n")); - enable_normal_ = node_->get_parameter("enable_normal").as_bool(); + enable_normal_color_ = node_->get_parameter("enable_normal_color").as_bool(); } From ca63a7a0fe5a990b843707576e5bf5c3bc29c8d6 Mon Sep 17 00:00:00 2001 From: HO JIN LEE Date: Mon, 9 Dec 2024 19:32:40 +0100 Subject: [PATCH 11/14] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index f2f8338d..9f0b0397 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ ## ROS2 Elevation Mapping Cupy - **Status**: Under Development 🚧 + **Status**: Image and Pointcloud subscribers done ### Features - **Point cloud-based map update**: *Functional* -- **Image-based map update**: *Ongoing development* +- **Image-based map update**: *Functional* ### Dependencies - - **ROS 2 Humble** From 5308b6d442270e880f17f3a52eb3d0aa03be2cdb Mon Sep 17 00:00:00 2001 From: HO JIN LEE Date: Mon, 9 Dec 2024 19:33:49 +0100 Subject: [PATCH 12/14] Update README.md --- README.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/README.md b/README.md index 9f0b0397..ac9a081a 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,10 @@ - **Point cloud-based map update**: *Functional* - **Image-based map update**: *Functional* + +![HILS in Jetson AGX Orin]https://github.com/user-attachments/assets/120d7f41-e54a-4fdf-af30-3e09cd5061a1 + + ### Dependencies - - **ROS 2 Humble** - **CUDA 12.4** From e52746cac08d06503001dc3c8cd914f7b680cc5e Mon Sep 17 00:00:00 2001 From: amilearning Date: Tue, 10 Dec 2024 18:18:47 +0100 Subject: [PATCH 13/14] no recordable --- .../config/core/core_param.yaml | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index 3a693641..cde8a252 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -112,7 +112,7 @@ elevation_mapping: topic_name: '/a200/sensors/camera_0/points' data_type: 'pointcloud' image1: - topic_name: '/feat_processing_node/semantic_seg_feat' + topic_name: '/feat_processing_node/semantic_feat' info_name: '/feat_processing_node/a200/sensors/camera_0/color/camera_info_resized' channel_name: '/feat_processing_node/feat_channel_info' data_type: 'image' @@ -142,17 +142,18 @@ elevation_mapping: publishers: elevation_map_raw: - layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2','feat_3','feat_4'] + # layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2','feat_3','feat_4','feat_5','feat_6','feat_7','feat_8','feat_9'] + layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2'] basic_layers: ['elevation'] fps: 5.0 - elevation_map_recordable: - layers: ['elevation', 'traversability'] - basic_layers: ['elevation', 'traversability'] - fps: 2.0 - elevation_map_filter: - layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] - basic_layers: ['min_filter'] - fps: 3.0 + # elevation_map_recordable: + # layers: ['elevation', 'traversability'] + # basic_layers: ['elevation', 'traversability'] + # fps: 2.0 + # elevation_map_filter: + # layers: ['min_filter', 'smooth', 'inpaint', 'elevation'] + # basic_layers: ['min_filter'] + # fps: 3.0 # plugun info From c68bbece2169c87a0da26e76b4d181bf8778a83f Mon Sep 17 00:00:00 2001 From: amilearning Date: Sun, 15 Dec 2024 00:38:46 +0100 Subject: [PATCH 14/14] add is_valid layer publisher --- elevation_mapping_cupy/config/core/core_param.yaml | 10 +++++----- .../elevation_mapping_cupy/elevation_mapping_ros.hpp | 1 + .../script/elevation_mapping_cupy/elevation_mapping.py | 6 ++++++ elevation_mapping_cupy/src/elevation_mapping_ros.cpp | 4 ++-- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/elevation_mapping_cupy/config/core/core_param.yaml b/elevation_mapping_cupy/config/core/core_param.yaml index cde8a252..c00f3290 100644 --- a/elevation_mapping_cupy/config/core/core_param.yaml +++ b/elevation_mapping_cupy/config/core/core_param.yaml @@ -1,13 +1,13 @@ elevation_mapping: ros__parameters: #### Basic parameters ######## - voxel_filter_size: 0.05 + voxel_filter_size: 0.1 enable_normal_arrow_publishing: true - cell_n: 40000 # number of cells in the map. + cell_n: 10000 # number of cells in the map. data_type: 'float32' # data type for the map. average_weight: 0.5 drift_compensation_variance_inlier: 0.1 - checker_layer: 'traversability' # layer name for checking the validity of the cell. + checker_layer: 'elevation' # layer name for checking the validity of the cell. min_filter_size: 5 # size of the filter for min filter. min_filter_iteration: 3 # number of iterations for min filter. initialized_variance: 10.0 # initial variance for each cell. @@ -87,7 +87,7 @@ elevation_mapping: pointcloud_channel_fusions: rgb: 'color' # 'color' fusion is used for the 'rgb' channel - default: 'average' # 'average' fusion is used for channels not listed here + default: 'averagelate' # 'average' fusion is used for channels not listed here image_channel_fusions: rgb: 'color' # 'color' fusion is used for the 'rgb' channel @@ -143,7 +143,7 @@ elevation_mapping: publishers: elevation_map_raw: # layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2','feat_3','feat_4','feat_5','feat_6','feat_7','feat_8','feat_9'] - layers: ['elevation', 'traversability', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2'] + layers: ['is_valid','elevation', 'variance','rgb', 'normal_x', 'normal_y', 'normal_z', 'feat_0','feat_1','feat_2'] basic_layers: ['elevation'] fps: 5.0 # elevation_map_recordable: diff --git a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp index aa843e8f..32838ab4 100644 --- a/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp +++ b/elevation_mapping_cupy/include/elevation_mapping_cupy/elevation_mapping_ros.hpp @@ -219,6 +219,7 @@ visualization_msgs::msg::Marker vectorToArrowMarker(const Eigen::Vector3d& start double positionAlpha_; double orientationAlpha_; double voxel_filter_size_; + pcl::VoxelGrid voxel_filter; double recordableFps_; std::atomic_bool enablePointCloudPublishing_; diff --git a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py index ea19064c..8c0fc575 100644 --- a/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py +++ b/elevation_mapping_cupy/script/elevation_mapping_cupy/elevation_mapping.py @@ -613,6 +613,10 @@ def get_elevation(self): """ return self.process_map_for_publish(self.elevation_map[0], fill_nan=True, add_z=True) + + def get_is_valid(self): + m = xp.where(self.elevation_map[2] > 0.5, 1.0, 0.0) + return m[1:-1, 1:-1] def get_variance(self): """Get the variance layer. @@ -741,6 +745,8 @@ def get_map_with_name_ref(self, name, data): if name == "elevation": m = self.get_elevation() use_stream = False + elif name == "is_valid": + m = self.get_is_valid() elif name == "variance": m = self.get_variance() elif name == "traversability": diff --git a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp index 1e3f5505..bd5eb984 100644 --- a/elevation_mapping_cupy/src/elevation_mapping_ros.cpp +++ b/elevation_mapping_cupy/src/elevation_mapping_ros.cpp @@ -422,7 +422,7 @@ void ElevationMappingNode::inputPointCloud(const sensor_msgs::msg::PointCloud2:: // apply the voxel filtering pcl::PCLPointCloud2::Ptr pcl_pc (new pcl::PCLPointCloud2()); - pcl::VoxelGrid voxel_filter; + // pcl::VoxelGrid voxel_filter; voxel_filter.setInputCloud(raw_pcl_pc); voxel_filter.setLeafSize(voxel_filter_size_,voxel_filter_size_,voxel_filter_size_); voxel_filter.filter(*pcl_pc); @@ -580,7 +580,7 @@ void ElevationMappingNode::imageCallback(const sensor_msgs::msg::Image::SharedPt channels = imageChannelReady_[key].first.channels; inputImage(image_msg, camera_info_msg, channels); } - RCLCPP_INFO(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); + RCLCPP_DEBUG(this->get_logger(), "ElevationMap imageChannelCallback processed an image in %f sec.", (this->now() - start).seconds()); }