From 85beed0bf310c91a5c6d8756be6fa440d4aadb22 Mon Sep 17 00:00:00 2001 From: Joseph Schornak Date: Fri, 17 May 2019 17:21:36 -0500 Subject: [PATCH] Initial node - builds under ROS1 --- CMakeLists.txt | 128 ++++++++++++++++++++++++++++++ package.xml | 38 +++++++++ src/yak_ros1_node.cpp | 166 +++++++++++++++++++++++++++++++++++++++ src/yak_ros2_node.cpp | 177 ++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 509 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 package.xml create mode 100644 src/yak_ros1_node.cpp create mode 100644 src/yak_ros2_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..0b4297d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,128 @@ +cmake_minimum_required(VERSION 3.5.0) +project(yak_ros) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(yak REQUIRED) + +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(cv_bridge REQUIRED) + +find_package(gl_depth_sim REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(OpenCV REQUIRED COMPONENTS core highgui) + + +if ($ENV{ROS_VERSION} VERSION_EQUAL "1") + # Build for ROS 1 using Catkin + find_package(catkin REQUIRED COMPONENTS + roscpp + tf2 + tf2_eigen + cv_bridge) + + catkin_package( + INCLUDE_DIRS + ${yak_INCLUDE_DIRS} + LIBRARIES + ${OpenCV_LIBS} + ${yak_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${tf2_LIBRARIES} + ${tf2_ros_LIBRARIES} + ${tf2_eigen_LIBRARIES} + ${cv_bridge_LIBRARIES} + CATKIN_DEPENDS + DEPENDS + yak + tf2_eigen + tf2 + ) + + add_executable(${PROJECT_NAME}_node + src/yak_ros1_node.cpp) + target_link_libraries(${PROJECT_NAME}_node + ${CATKIN_LIBRARIES} + ${OpenCV_LIBS} + ${yak_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${tf2_LIBRARIES} + ${tf2_ros_LIBRARIES} + ${tf2_eigen_LIBRARIES} + ${cv_bridge_LIBRARIES} + ) + + target_include_directories(${PROJECT_NAME}_node PUBLIC + ${catkin_INCLUDE_DIRS} + ${yak_INCLUDE_DIRS} + ${gl_depth_sim_INCLUDE_DIRS} + ) + +else() + # Build for ROS 2 using Ament + find_package(ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) + + add_executable(${PROJECT_NAME}_node + src/yak_ros2_node.cpp) + target_link_libraries(${PROJECT_NAME}_node + ${rclcpp_LIBRARIES} + ${OpenCV_LIBS} + ${yak_LIBRARIES} + ${sensor_msgs_LIBRARIES} + ${tf2_LIBRARIES} + ${tf2_ros_LIBRARIES} + ${tf2_eigen_LIBRARIES} + ${cv_bridge_LIBRARIES} + ) + + ament_target_dependencies(${PROJECT_NAME}_node + "rclcpp" + "yak" + "std_srvs" + "sensor_msgs" + "geometry_msgs" + "tf2" + "tf2_ros" + "tf2_eigen" + ) + + ament_export_include_directories(include + ${rclcpp_INCLUDE_DIRS} + ${yak_INCLUDE_DIRS} + ) + + ament_export_dependencies(rclcpp yak tf2_eigen tf2) + + ament_package() +endif() + +if ($ENV{ROS_VERSION} VERSION_EQUAL "1") + set(ROS_LIB_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + set(ROS_BIN_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +else() + set(ROS_LIB_DESTINATION lib) + set(ROS_BIN_DESTINATION bin) +endif() + +set(ROS_INCLUDE_DESTINATION include) + +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION ${ROS_INCLUDE_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME}_node + RUNTIME DESTINATION ${ROS_LIB_DESTINATION} + ) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..f31ef26 --- /dev/null +++ b/package.xml @@ -0,0 +1,38 @@ + + + yak_ros + 0.1.0 + An example ROS node for yak + Joseph Schornak + Jonathan Meyer + Joseph Schornak + MIT + + catkin + ament_cmake + + gl_depth_sim + + roscpp + rclcpp + + + libpcl-all-dev + OpenMP + Eigen3 + yak + + sensor_msgs + geometry_msgs + std_srvs + + tf2 + tf2_ros + tf2_eigen + + + catkin + ament_cmake + + + diff --git a/src/yak_ros1_node.cpp b/src/yak_ros1_node.cpp new file mode 100644 index 0000000..8e5bb28 --- /dev/null +++ b/src/yak_ros1_node.cpp @@ -0,0 +1,166 @@ +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include + +static const std::double_t DEFAULT_MINIMUM_TRANSLATION = 0.00001; + +/** + * @brief The OnlineFusionServer class. Integrate depth images into a TSDF volume. When requested, mesh the volume using marching cubes. + * Note that this will work using both simulated and real robots and depth cameras. + */ +class OnlineFusionServer +{ +public: + /** + * @brief OnlineFusionServer constructor + * @param nh - ROS node handle + * @param params - KinFu parameters such as TSDF volume size, resolution, etc. + * @param world_to_volume - Transform from world frame to volume origin frame. + */ + OnlineFusionServer(ros::NodeHandle &nh, const kfusion::KinFuParams& params, const Eigen::Affine3f& world_to_volume) : + fusion_(params, world_to_volume), + params_(params), + robot_tform_listener_(tf_buffer_), + world_to_camera_prev_(Eigen::Affine3d::Identity()) + { + // Subscribe to depth images published on the topic named by the depth_topic param. Set up callback to integrate images when received. + std::string depth_topic; + nh.getParam("depth_topic", depth_topic); + point_cloud_sub_ = nh.subscribe(depth_topic, 1, &OnlineFusionServer::onReceivedDepthImg, this); + + // Advertise service for marching cubes meshing + generate_mesh_service_ = nh.advertiseService("generate_mesh_service", &OnlineFusionServer::onGenerateMesh, this); + } + +private: + /** + * @brief onReceivedDepthImg - callback for integrating new depth images into the TSDF volume + * @param image_in - pointer to new depth image + */ + void onReceivedDepthImg(const sensor_msgs::ImageConstPtr& image_in) + { + // Get the camera pose in the world frame at the time when the depth image was generated. + ROS_INFO_STREAM("Got depth image"); + geometry_msgs::TransformStamped transform_world_to_camera; + try + { + transform_world_to_camera = tf_buffer_.lookupTransform("base_link", image_in->header.frame_id, image_in->header.stamp); + } + catch(tf2::TransformException &ex) + { + // Abort integration if tf lookup failed + ROS_WARN("%s", ex.what()); + return; + } + Eigen::Affine3d world_to_camera = tf2::transformToEigen(transform_world_to_camera); + + // Find how much the camera moved since the last depth image. If the magnitude of motion was below some threshold, abort integration. + // This is to prevent noise from accumulating in the isosurface due to numerous observations from the same pose. + std::double_t motion_mag = (world_to_camera.inverse() * world_to_camera_prev_).translation().norm(); + ROS_INFO_STREAM(motion_mag); + if(motion_mag < DEFAULT_MINIMUM_TRANSLATION) + { + ROS_INFO_STREAM("Camera motion below threshold"); + return; + } + + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_in, sensor_msgs::image_encodings::TYPE_16UC1); + + // Integrate the depth image into the TSDF volume + if (!fusion_.fuse(cv_ptr->image, world_to_camera.cast())) + ROS_WARN_STREAM("Failed to fuse image"); + + // If integration was successful, update the previous camera pose with the new camera pose + world_to_camera_prev_ = world_to_camera; + return; + } + + /** + * @brief onGenerateMesh - Perform marching cubes meshing on the TSDF volume and save the result as a binary .ply file. + * @param req + * @param res + * @return + */ + bool onGenerateMesh(std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) + { + yak::MarchingCubesParameters mc_params; + mc_params.scale = params_.volume_resolution; + pcl::PolygonMesh mesh = yak::marchingCubesCPU(fusion_.downloadTSDF(), mc_params); + ROS_INFO_STREAM("Meshing done, saving ply"); + pcl::io::savePLYFileBinary("cubes.ply", mesh); + ROS_INFO_STREAM("Saving done"); + res.success = true; + return true; + } + + ros::Subscriber point_cloud_sub_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener robot_tform_listener_; + ros::ServiceServer generate_mesh_service_; + yak::FusionServer fusion_; + const kfusion::KinFuParams params_; + Eigen::Affine3d world_to_camera_prev_; +}; + +/** + * @brief main - Initialize the tsdf_node + * @param argc + * @param argv + * @return + */ +int main(int argc, char **argv) +{ + ros::init(argc, argv, "tsdf_node"); + ros::NodeHandle nh("tsdf_node"); + + kfusion::KinFuParams default_params = kfusion::KinFuParams::default_params(); + default_params.use_pose_hints = true; // use robot forward kinematics to find camera pose relative to TSDF volume + default_params.use_icp = false; // since we're using robot FK to get the camera pose, don't use ICP (TODO: yet!) + default_params.update_via_sensor_motion = false; + + // Get camera intrinsics from params + XmlRpc::XmlRpcValue camera_matrix; + nh.getParam("camera/camera_matrix/data", camera_matrix); + default_params.intr.fx = static_cast(camera_matrix[0]); + default_params.intr.fy = static_cast(camera_matrix[4]); + default_params.intr.cx = static_cast(camera_matrix[2]); + default_params.intr.cy = static_cast(camera_matrix[5]); + + ROS_INFO("Camera Intr Params: %f %f %f %f\n", default_params.intr.fx, default_params.intr.fy, default_params.intr.cx, default_params.intr.cy); + + + // TODO: Don't hardcode TSDF volume origin pose + Eigen::Affine3f world_to_volume (Eigen::Affine3f::Identity()); + world_to_volume.translation() += Eigen::Vector3f(0.0f, -1.5f, -2.0f); + + // Set up TSDF parameters + // TODO: Autocompute resolution from volume length/width/height in meters + default_params.volume_dims = cv::Vec3i(640, 640, 640); + default_params.volume_resolution = 0.005; + default_params.volume_pose = Eigen::Affine3f::Identity(); // This gets overwritten when Yak is initialized + default_params.tsdf_trunc_dist = default_params.volume_resolution * 5.0f; //meters; + default_params.tsdf_max_weight = 50; //frames + default_params.raycast_step_factor = 0.25; //in voxel sizes + default_params.gradient_delta_factor = 0.25; //in voxel sizes + + // Set up the fusion server with the above parameters; + OnlineFusionServer ofs(nh, default_params, world_to_volume); + + // Do TSDF-type things for the lifetime of the node + ros::spin(); + return 0; +} diff --git a/src/yak_ros2_node.cpp b/src/yak_ros2_node.cpp new file mode 100644 index 0000000..3ff3720 --- /dev/null +++ b/src/yak_ros2_node.cpp @@ -0,0 +1,177 @@ +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include + +static const std::string DEFAULT_DEPTH_IMAGE_TOPIC = "/camera/depth/image_rect_raw"; +static const std::double_t DEFAULT_MINIMUM_TRANSLATION = 0.00001; + +/** + * @brief The Fusion class. Integrate depth images into a TSDF volume. When requested, mesh the volume using marching cubes. + * Note that this will work using both simulated and real robots and depth cameras. + */ +class Fusion +{ +public: + + /** + * @brief OnlineFusionServer constructor + * @param nh - ROS node handle + * @param params - KinFu parameters such as TSDF volume size, resolution, etc. + * @param world_to_volume - Transform from world frame to volume origin frame. + * @param tsdf_base_frame - The name of the tf frame that will be used as the base frame when performing lookups between the TSDF volume and the camera frame. + */ + explicit Fusion(const rclcpp::Node::SharedPtr node, const kfusion::KinFuParams& params, const Eigen::Affine3f& world_to_volume, const std::string tsdf_base_frame) : + node_(node), + fusion_(params, world_to_volume), + params_(params), + clock_(std::make_shared(RCL_SYSTEM_TIME)), + tf_buffer_(clock_), + robot_tform_listener_(tf_buffer_), + world_to_camera_prev_(Eigen::Affine3d::Identity()), + tsdf_base_frame_(tsdf_base_frame) + { + // Subscribe to depth images published on the topic named by the depth_topic param. Set up callback to integrate images when received. + std::string depth_topic = DEFAULT_DEPTH_IMAGE_TOPIC; + + auto depth_image_cb = [this](const sensor_msgs::msg::Image::SharedPtr image_in) -> void + { + // Get the camera pose in the world frame at the time when the depth image was generated. + RCLCPP_INFO(node_->get_logger(), "Got depth image"); + geometry_msgs::msg::TransformStamped transform_world_to_camera; + try + { + transform_world_to_camera = tf_buffer_.lookupTransform(tsdf_base_frame_, image_in->header.frame_id, tf2::TimePoint(std::chrono::seconds(image_in->header.stamp.sec) + std::chrono::nanoseconds(image_in->header.stamp.nanosec)), tf2::Duration(1000000000)); + // transform_world_to_camera = tf_buffer_.lookupTransform("table", "camera_base_link", tf2::TimePoint(std::chrono::seconds(image_in->header.stamp.sec) + std::chrono::nanoseconds(image_in->header.stamp.nanosec))); + } + catch(tf2::TransformException &ex) + { + // Abort integration if tf lookup failed + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + return; + } + Eigen::Affine3d world_to_camera = tf2::transformToEigen(transform_world_to_camera); + + // Find how much the camera moved since the last depth image. If the magnitude of motion was below some threshold, abort integration. + // This is to prevent noise from accumulating in the isosurface due to numerous observations from the same pose. + std::double_t motion_mag = (world_to_camera.inverse() * world_to_camera_prev_).translation().norm(); + + if(motion_mag < DEFAULT_MINIMUM_TRANSLATION) + { + RCLCPP_INFO(node_->get_logger(), "Camera motion below threshold"); + return; + } + + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_in, sensor_msgs::image_encodings::TYPE_16UC1); + + // Integrate the depth image into the TSDF volume + if (!fusion_.fuse(cv_ptr->image, world_to_camera.cast())) + { + RCLCPP_WARN(node_->get_logger(), "Failed to fuse image"); + } + + // If integration was successful, update the previous camera pose with the new camera pose + world_to_camera_prev_ = world_to_camera; + return; + }; + + depth_image_sub_ = node_->create_subscription(depth_topic, depth_image_cb); + + auto generate_mesh_cb = + [this](const std::shared_ptr request_header, + const std::shared_ptr req, + const std::shared_ptr res) -> void + { + RCLCPP_INFO(node_->get_logger(), "Starting mesh generation"); + yak::MarchingCubesParameters mc_params; + mc_params.scale = params_.volume_resolution; + pcl::PolygonMesh mesh = yak::marchingCubesCPU(fusion_.downloadTSDF(), mc_params); + RCLCPP_INFO(node_->get_logger(), "Meshing done, saving ply"); + pcl::io::savePLYFileBinary("cubes.ply", mesh); + RCLCPP_INFO(node_->get_logger(), "Saving done"); + }; + + // Advertise service for marching cubes meshing + generate_mesh_service_ = node_->create_service("generate_mesh_service", generate_mesh_cb); + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Clock::SharedPtr clock_; + rclcpp::Subscription::SharedPtr depth_image_sub_; + rclcpp::Service::SharedPtr generate_mesh_service_; + + std::string tsdf_base_frame_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener robot_tform_listener_; + + yak::FusionServer fusion_; + const kfusion::KinFuParams params_; + Eigen::Affine3d world_to_camera_prev_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("fusion_node"); + + std::string tsdf_base_frame; + node->get_parameter_or("tsdf_base_frame", tsdf_base_frame, "turntable_platform"); + + // Set up TSDF parameters + kfusion::KinFuParams default_params = kfusion::KinFuParams::default_params(); + + node->get_parameter_or("kinfu_params.use_pose_hints", default_params.use_pose_hints, true); // use robot forward kinematics to find camera pose relative to TSDF volume + node->get_parameter_or("kinfu_params.use_icp", default_params.use_icp, false); // since we're using robot FK to get the camera pose, don't use ICP (TODO: yet!) + node->get_parameter_or("kinfu_params.update_via_sensor_motion", default_params.update_via_sensor_motion, false); // deprecated? + + node->get_parameter_or("kinfu_params.intr.fx", default_params.intr.fx, 380.4028625488281f); + node->get_parameter_or("kinfu_params.intr.fy", default_params.intr.fy, 380.81707763671875f); + node->get_parameter_or("kinfu_params.intr.cx", default_params.intr.cx, 316.48406982421875f); + node->get_parameter_or("kinfu_params.intr.fy", default_params.intr.cy, 245.3903045654297f); + + float world_to_vol_x, world_to_vol_y, world_to_vol_z; + node->get_parameter_or("kinfu_params.world_to_volume.x", world_to_vol_x, -0.5f); + node->get_parameter_or("kinfu_params.world_to_volume.y", world_to_vol_y, -0.5f); + node->get_parameter_or("kinfu_params.world_to_volume.z", world_to_vol_z, -0.25f); + Eigen::Affine3f world_to_volume (Eigen::Affine3f::Identity()); + world_to_volume.translation() += Eigen::Vector3f(world_to_vol_x, world_to_vol_y, world_to_vol_z); + + int voxels_x, voxels_y, voxels_z; + node->get_parameter_or("kinfu_params.volume_dims_voxels.x", voxels_x, 320); + node->get_parameter_or("kinfu_params.volume_dims_voxels.y", voxels_y, 320); + node->get_parameter_or("kinfu_params.volume_dims_voxels.z", voxels_z, 320); + + double voxel_resolution; + node->get_parameter_or("kinfu_params.voxel_resolution_meters", voxel_resolution, 0.003); + + // TODO: Autocompute resolution from volume length/width/height in meters + default_params.volume_dims = cv::Vec3i(voxels_x, voxels_y, voxels_z); + default_params.volume_resolution = voxel_resolution; + default_params.volume_pose = Eigen::Affine3f::Identity(); // This gets overwritten when Yak is initialized + default_params.tsdf_trunc_dist = default_params.volume_resolution * 5.0f; //meters; + default_params.tsdf_max_weight = 50; //frames + default_params.raycast_step_factor = 0.25; //in voxel sizes + default_params.gradient_delta_factor = 0.25; //in voxel sizes + + RCLCPP_INFO(node->get_logger(), "Starting fusion node"); + + auto fusion = std::make_shared(node, default_params, world_to_volume, tsdf_base_frame); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} +