diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index c74387d..7ff6112 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -49,6 +49,7 @@ add_library( src/input/ros_data_receiver.cpp src/input/ros_input_module.cpp src/input/ros_sensors.cpp + src/places/external_traversability_estimator.cpp src/odometry/ros_pose_graph_tracker.cpp src/openset/ros_embedding_group.cpp src/utils/bow_subscriber.cpp diff --git a/hydra_ros/include/hydra_ros/input/image_receiver.h b/hydra_ros/include/hydra_ros/input/image_receiver.h index c81e786..04a224e 100644 --- a/hydra_ros/include/hydra_ros/input/image_receiver.h +++ b/hydra_ros/include/hydra_ros/input/image_receiver.h @@ -182,6 +182,30 @@ void ImageReceiverImpl::callback( queue.push(packet); } +class RGBDImageReceiver : public RosDataReceiver { + public: + struct Config : RosDataReceiver::Config {}; + using Policy = + message_filters::sync_policies::ApproximateTime; + using Synchronizer = message_filters::Synchronizer; + + RGBDImageReceiver(const Config& config, const std::string& sensor_name); + virtual ~RGBDImageReceiver() = default; + + protected: + bool initImpl() override; + + void callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, + const sensor_msgs::msg::Image::ConstSharedPtr& depth); + + ColorSubscriber color_sub_; + DepthSubscriber depth_sub_; + std::unique_ptr sync_; +}; + +void declare_config(RGBDImageReceiver::Config& config); + class ClosedSetImageReceiver : public ImageReceiverImpl { public: struct Config : RosDataReceiver::Config {}; diff --git a/hydra_ros/include/hydra_ros/places/external_traversability_estimator.h b/hydra_ros/include/hydra_ros/places/external_traversability_estimator.h new file mode 100644 index 0000000..a8cea81 --- /dev/null +++ b/hydra_ros/include/hydra_ros/places/external_traversability_estimator.h @@ -0,0 +1,81 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +#include + +#include + +#include +#include + +namespace hydra::places { + +/** + * @brief Takes in an external traversability map as occupancy grid to perform + * traversability analysis. + */ +class ExternalTraversabilityEstimator : public TraversabilityEstimator { + public: + struct Config { + //! @brief The height above the robot body to consider for traversability in meters. + std::string input_topic = "/local_cost_map"; + unsigned int queue_size = 2; + } const config; + + using State = spark_dsg::TraversabilityState; + + ExternalTraversabilityEstimator(const Config& config); + ~ExternalTraversabilityEstimator() override = default; + + void updateTraversability(const ActiveWindowOutput& msg, + const kimera_pgmo::MeshDelta& /* mesh_delta */, + const spark_dsg::DynamicSceneGraph& /* graph */) override; + + void callback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg); + + const TraversabilityLayer& getTraversabilityLayer() const override; + + protected: + rclcpp::Subscription::SharedPtr sub_; + mutable std::mutex mutex_; + + protected: + State occupancyToTraversability(int8_t occupancy) const; +}; + +void declare_config(ExternalTraversabilityEstimator::Config& config); + +} // namespace hydra::places diff --git a/hydra_ros/src/frontend/traversability_visualizer.cpp b/hydra_ros/src/frontend/traversability_visualizer.cpp index 13f93d2..968edcc 100644 --- a/hydra_ros/src/frontend/traversability_visualizer.cpp +++ b/hydra_ros/src/frontend/traversability_visualizer.cpp @@ -173,7 +173,7 @@ Color TraversabilityVisualizer::debugColor(float value) const { if (value < 0.0f) { return Color::black(); } - return spark_dsg::colormaps::rainbowId(static_cast(value), 5); + return spark_dsg::colormaps::rainbowId(static_cast(value), 10); } } // namespace hydra diff --git a/hydra_ros/src/input/image_receiver.cpp b/hydra_ros/src/input/image_receiver.cpp index c46e092..85403a1 100644 --- a/hydra_ros/src/input/image_receiver.cpp +++ b/hydra_ros/src/input/image_receiver.cpp @@ -56,11 +56,28 @@ ColorSubscriber::Filter& ColorSubscriber::getFilter() const { } void ColorSubscriber::fillInput(const Image& img, ImageInputPacket& packet) const { - try { - packet.color = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image; - } catch (const cv_bridge::Exception& e) { - LOG(ERROR) << "Failed to convert color image: " << e.what(); + // Allow also mono images to be converted to grayscale. + if (sensor_msgs::image_encodings::isColor(img.encoding)) { + try { + packet.color = + cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::RGB8)->image; + return; + } catch (const cv_bridge::Exception& e) { + LOG(ERROR) << "Failed to convert mono image as color input: " << e.what(); + return; + } + } else if (sensor_msgs::image_encodings::isMono(img.encoding)) { + try { + cv::Mat mono = + cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8)->image; + cv::cvtColor(mono, packet.color, cv::COLOR_GRAY2RGB); + return; + } catch (const cv_bridge::Exception& e) { + LOG(ERROR) << "Failed to convert color image: " << e.what(); + return; + } } + LOG(ERROR) << "Failed to convert color image: unsupported encoding: " << img.encoding; } DepthSubscriber::DepthSubscriber() = default; @@ -130,6 +147,38 @@ void FeatureSubscriber::fillInput(const MsgType& msg, ImageInputPacket& packet) } } +void declare_config(RGBDImageReceiver::Config& config) { + using namespace config; + name("RGBDImageReceiver::Config"); + base(config); +} + +RGBDImageReceiver::RGBDImageReceiver(const Config& config, + const std::string& sensor_name) + : RosDataReceiver(config, sensor_name) {} + +bool RGBDImageReceiver::initImpl() { + color_sub_ = ColorSubscriber(ianvs::NodeHandle::this_node(ns_)); + depth_sub_ = DepthSubscriber(ianvs::NodeHandle::this_node(ns_)); + sync_.reset(new Synchronizer( + Policy(config.queue_size), color_sub_.getFilter(), depth_sub_.getFilter())); + sync_->registerCallback(&RGBDImageReceiver::callback, this); + return true; +} + +void RGBDImageReceiver::callback(const sensor_msgs::msg::Image::ConstSharedPtr& color, + const sensor_msgs::msg::Image::ConstSharedPtr& depth) { + const auto timestamp_ns = rclcpp::Time(color->header.stamp).nanoseconds(); + if (!checkInputTimestamp(timestamp_ns)) { + return; + } + + auto packet = std::make_shared(timestamp_ns, sensor_name_); + color_sub_.fillInput(*color, *packet); + depth_sub_.fillInput(*depth, *packet); + queue.push(packet); +} + void declare_config(ClosedSetImageReceiver::Config& config) { using namespace config; name("ClosedSetImageReceiver::Config"); @@ -152,6 +201,12 @@ OpenSetImageReceiver::OpenSetImageReceiver(const Config& config, namespace { +static const auto no_semantic_registration = + config::RegistrationWithConfig("RGBDImageReceiver"); + static const auto closed_registration = config::RegistrationWithConfig +#include +#include +#include + +namespace hydra::places { + +using spark_dsg::TraversabilityState; + +static const auto registration = + config::RegistrationWithConfig( + "ExternalTraversabilityEstimator"); + +void declare_config(ExternalTraversabilityEstimator::Config& config) { + using namespace config; + name("ExternalTraversabilityEstimator::Config"); + field(config.input_topic, "input_topic"); + field(config.queue_size, "queue_size"); + checkCondition(!config.input_topic.empty(), "'input_topic' must be specified"); +} + +ExternalTraversabilityEstimator::ExternalTraversabilityEstimator(const Config& config) + : config(config::checkValid(config)) { + auto nh = ianvs::NodeHandle::this_node(); + sub_ = nh.create_subscription( + config.input_topic, + config.queue_size, + &ExternalTraversabilityEstimator::callback, + this); +} + +const TraversabilityLayer& ExternalTraversabilityEstimator::getTraversabilityLayer() + const { + std::lock_guard lock(mutex_); + return *traversability_layer_; +} + +void ExternalTraversabilityEstimator::updateTraversability( + const ActiveWindowOutput& msg, + const kimera_pgmo::MeshDelta&, + const spark_dsg::DynamicSceneGraph&) { + if (!traversability_layer_) { + const auto& map_config = msg.map().config; + traversability_layer_ = std::make_unique( + map_config.voxel_size, map_config.voxels_per_side); + } +} + +void ExternalTraversabilityEstimator::callback( + const nav_msgs::msg::OccupancyGrid::SharedPtr msg) { + if (!traversability_layer_) { + return; + } + // Reset update tracking. + std::lock_guard lock(mutex_); + for (auto& block : *traversability_layer_) { + block.updated = false; + for (auto& voxel : block.voxels) { + voxel.confidence = 0.0f; + } + } + + // Updated the stored layer with the map. + const float voxel_size = msg->info.resolution; + const Eigen::Vector3f origin( + msg->info.origin.position.x, msg->info.origin.position.y, 0.0f); + for (unsigned int y = 0; y < msg->info.height; ++y) { + for (unsigned int x = 0; x < msg->info.width; ++x) { + const int idx = x + y * msg->info.width; + const State state = occupancyToTraversability(msg->data[idx]); + // Compute the voxel coordinates. + const Eigen::Vector3f position = + Eigen::Vector3f(x, y, 0.0f) * voxel_size + origin; + const auto index = traversability_layer_->globalIndexFromPoint(position); + // TODO(lschmid): Fix the allocate block interfaces for the traversability layer. + auto& block = traversability_layer_->allocateBlock( + traversability_layer_->blockIndexFromGlobal(index), + traversability_layer_->voxels_per_side); + block.updated = true; + auto& voxel = block.voxel(traversability_layer_->voxelIndexFromGlobal(index)); + if (voxel.confidence == 0.0f) { + // Overwrite if this is the first observation this iteration. + voxel.state = state; + voxel.confidence = 1.0f; + } else { + spark_dsg::fuseStates(state, voxel.state); + voxel.confidence += 1.0f; + } + } + } + + // Remove blocks that were not updated. + spatial_hash::BlockIndices to_remove; + for (auto& block : *traversability_layer_) { + if (!block.updated) { + to_remove.push_back(block.index); + } + } + traversability_layer_->removeBlocks(to_remove); +} + +ExternalTraversabilityEstimator::State +ExternalTraversabilityEstimator::occupancyToTraversability(int8_t occupancy) const { + if (occupancy < 0) { + return State::UNKNOWN; + } + if (occupancy == 0) { + return State::TRAVERSABLE; + } + return State::INTRAVERSABLE; +} + +} // namespace hydra::places diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h index 8542bc1..f959fa9 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/khronos_object_plugin.h @@ -73,6 +73,7 @@ #pragma once +#include #include #include #include @@ -122,7 +123,7 @@ class KhronosObjectPlugin : public VisualizerPlugin { //! Layer to draw objects for std::string layer = spark_dsg::DsgLayers::OBJECTS; - } const config; + }; // Construction. KhronosObjectPlugin(const Config& config, @@ -136,9 +137,11 @@ class KhronosObjectPlugin : public VisualizerPlugin { protected: // Helper functions. - void drawDynamicObjects(const std_msgs::msg::Header& header, + void drawDynamicObjects(const Config& config, + const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph); - void drawStaticObjects(const std_msgs::msg::Header& header, + void drawStaticObjects(const Config& config, + const std_msgs::msg::Header& header, const spark_dsg::DynamicSceneGraph& graph); // Mesh namespace for the visualizer plugin. @@ -153,13 +156,15 @@ class KhronosObjectPlugin : public VisualizerPlugin { const uint64_t id); // Get the color of a dynamic object. - spark_dsg::Color getDynamicColor(const spark_dsg::KhronosObjectAttributes& attrs, + spark_dsg::Color getDynamicColor(const Config& config, + const spark_dsg::KhronosObjectAttributes& attrs, const uint64_t id) const; // Clear static object visuliazation void resetObject(const std_msgs::msg::Header& header, const uint64_t id); private: + const config::DynamicConfig config_; // ROS. rclcpp::Publisher::SharedPtr dynamic_pub_; rclcpp::Publisher::SharedPtr static_pub_; diff --git a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h index 70e0cdc..a68f060 100644 --- a/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h +++ b/hydra_visualizer/include/hydra_visualizer/plugins/mesh_plugin.h @@ -33,13 +33,12 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once +#include #include #include #include #include -#include -#include #include "hydra_visualizer/adapters/mesh_color.h" #include "hydra_visualizer/plugins/visualizer_plugin.h" @@ -52,9 +51,8 @@ class MeshPlugin : public VisualizerPlugin { using LabelsPtr = std::shared_ptr; struct Config { - bool use_color_adapter = false; - config::VirtualConfig coloring{SemanticMeshColoring::Config()}; - } const config; + config::VirtualConfig coloring; + }; MeshPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name); @@ -66,15 +64,11 @@ class MeshPlugin : public VisualizerPlugin { void reset(const std_msgs::msg::Header& header) override; protected: - void handleService(const std_srvs::srv::SetBool::Request::SharedPtr& req, - std_srvs::srv::SetBool::Response::SharedPtr res); + config::DynamicConfig config_; std::string getMsgNamespace() const; - bool use_color_adapter_; rclcpp::Publisher::SharedPtr mesh_pub_; - rclcpp::Service::SharedPtr toggle_service_; - std::shared_ptr mesh_coloring_; }; void declare_config(MeshPlugin::Config& config); diff --git a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp index b048848..5261eb7 100644 --- a/hydra_visualizer/src/plugins/khronos_object_plugin.cpp +++ b/hydra_visualizer/src/plugins/khronos_object_plugin.cpp @@ -122,7 +122,7 @@ KhronosObjectPlugin::KhronosObjectPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name) : VisualizerPlugin(name), - config(config::checkValid(config)), + config_("KhronosObjectPlugin", config::checkValid(config)), dynamic_pub_( nh.create_publisher("dynamic_objects", config.queue_size)), static_pub_(nh.create_publisher("static_objects", @@ -131,11 +131,12 @@ KhronosObjectPlugin::KhronosObjectPlugin(const Config& config, void KhronosObjectPlugin::draw(const std_msgs::msg::Header& header, const DynamicSceneGraph& graph) { + const auto config = config_.get(); if (!graph.hasLayer(config.layer)) { return; } - drawDynamicObjects(header, graph); - drawStaticObjects(header, graph); + drawDynamicObjects(config, header, graph); + drawStaticObjects(config, header, graph); } void KhronosObjectPlugin::reset(const std_msgs::msg::Header& header) { @@ -153,7 +154,8 @@ void KhronosObjectPlugin::reset(const std_msgs::msg::Header& header) { previous_objects_.clear(); } -void KhronosObjectPlugin::drawDynamicObjects(const std_msgs::msg::Header& header, +void KhronosObjectPlugin::drawDynamicObjects(const Config& config, + const std_msgs::msg::Header& header, const DynamicSceneGraph& graph) { if (dynamic_pub_->get_subscription_count() == 0) { return; @@ -171,7 +173,7 @@ void KhronosObjectPlugin::drawDynamicObjects(const std_msgs::msg::Header& header const uint64_t id = spark_dsg::NodeSymbol(node_id).categoryId(); spark_dsg::BoundingBox bbox = attrs->bounding_box; - const auto color = visualizer::makeColorMsg(getDynamicColor(*attrs, id)); + const auto color = visualizer::makeColorMsg(getDynamicColor(config, *attrs, id)); Marker bbox_template; bbox_template.type = Marker::LINE_LIST; @@ -220,7 +222,8 @@ void KhronosObjectPlugin::drawDynamicObjects(const std_msgs::msg::Header& header dynamic_pub_->publish(msg); } -void KhronosObjectPlugin::drawStaticObjects(const std_msgs::msg::Header& header, +void KhronosObjectPlugin::drawStaticObjects(const Config& config, + const std_msgs::msg::Header& header, const DynamicSceneGraph& dsg) { if (static_pub_->get_subscription_count() == 0) { return; @@ -295,7 +298,8 @@ void KhronosObjectPlugin::publishTransform(const std_msgs::msg::Header& header, tf_broadcaster_.sendTransform(msg); } -Color KhronosObjectPlugin::getDynamicColor(const KhronosObjectAttributes& attrs, +Color KhronosObjectPlugin::getDynamicColor(const Config& config, + const KhronosObjectAttributes& attrs, const uint64_t id) const { switch (config.dynamic_color_mode) { case Config::DynamicColorMode::ID: diff --git a/hydra_visualizer/src/plugins/mesh_plugin.cpp b/hydra_visualizer/src/plugins/mesh_plugin.cpp index e0b164c..dffffe3 100644 --- a/hydra_visualizer/src/plugins/mesh_plugin.cpp +++ b/hydra_visualizer/src/plugins/mesh_plugin.cpp @@ -58,10 +58,6 @@ using spark_dsg::DynamicSceneGraph; void declare_config(MeshPlugin::Config& config) { using namespace config; name("MeshPlugin::Config"); - // TODO(lschmid): Not the most elegant, would be nice to dynamically set different - // colorings once config_utilities dynamic config is ready. - field(config.use_color_adapter, "use_color_adapter"); - config.coloring.setOptional(); field(config.coloring, "coloring"); } @@ -69,16 +65,9 @@ MeshPlugin::MeshPlugin(const Config& config, ianvs::NodeHandle nh, const std::string& name) : VisualizerPlugin(name), - config(config::checkValid(config)), - use_color_adapter_(config.use_color_adapter), + config_("MeshPlugin", config::checkValid(config)), mesh_pub_(nh.create_publisher( - name, rclcpp::QoS(1).transient_local())), - mesh_coloring_(config.coloring.create()) { - if (mesh_coloring_) { - toggle_service_ = (nh / name).create_service( - "use_color_adapter", &MeshPlugin::handleService, this); - } -} + name, rclcpp::QoS(1).transient_local())) {} MeshPlugin::~MeshPlugin() {} @@ -89,13 +78,10 @@ void MeshPlugin::draw(const std_msgs::msg::Header& header, return; } - if (use_color_adapter_ && !mesh_coloring_) { - LOG(WARNING) - << "[MeshPlugin] Invalid colormap; defaulting to original vertex color"; - } + const auto config = config_.get(); auto msg = visualizer::makeMeshMsg( - header, *mesh, getMsgNamespace(), use_color_adapter_ ? mesh_coloring_ : nullptr); + header, *mesh, getMsgNamespace(), config.coloring.create()); mesh_pub_->publish(msg); } @@ -111,11 +97,4 @@ std::string MeshPlugin::getMsgNamespace() const { return "robot0/dsg_mesh"; } -void MeshPlugin::handleService(const std_srvs::srv::SetBool::Request::SharedPtr& req, - std_srvs::srv::SetBool::Response::SharedPtr res) { - use_color_adapter_ = req->data; - res->success = true; - has_change_ = true; -} - } // namespace hydra