diff --git a/src/perception/rai_whatisee/CMakeLists.txt b/src/perception/rai_whatisee/CMakeLists.txt new file mode 100644 index 00000000..216338cd --- /dev/null +++ b/src/perception/rai_whatisee/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_minimum_required(VERSION 3.8) +project(rai_whatisee) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Determine ROS 2 distribution +if(DEFINED ENV{ROS_DISTRO}) + if($ENV{ROS_DISTRO} STREQUAL "humble") + add_definitions(-DROS_DISTRO_HUMBLE) + elseif($ENV{ROS_DISTRO} STREQUAL "jazzy") + add_definitions(-DROS_DISTRO_JAZZY) + else() + message(WARNING "Unsupported ROS 2 distribution: $ENV{ROS_DISTRO}. Defaulting to .hpp include.") + endif() +else() + message(FATAL_ERROR "ROS_DISTRO environment variable not set. Make sure to source your ROS 2 distribution before compiling.") +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rai_interfaces REQUIRED) +find_package(std_srvs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) + +add_executable(rai_whatisee_node src/rai_whatisee_node.cpp) +target_include_directories(rai_whatisee_node PUBLIC + $ + $) +target_compile_features(rai_whatisee_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(rai_whatisee_node rclcpp std_srvs rai_interfaces cv_bridge OpenCV) + +install(TARGETS rai_whatisee_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/perception/rai_whatisee/LICENSE b/src/perception/rai_whatisee/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/perception/rai_whatisee/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/perception/rai_whatisee/package.xml b/src/perception/rai_whatisee/package.xml new file mode 100644 index 00000000..8c125db8 --- /dev/null +++ b/src/perception/rai_whatisee/package.xml @@ -0,0 +1,24 @@ + + + + rai_whatisee + 0.1.0 + A package that handles robot perception in a way that is best suited for AI Agents + Adam Dabrowski + Apache-2.0 + + ament_cmake + rclcpp + rai_interfaces + std_srvs + cv_bridge + + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/perception/rai_whatisee/src/rai_whatisee_node.cpp b/src/perception/rai_whatisee/src/rai_whatisee_node.cpp new file mode 100644 index 00000000..c911dc32 --- /dev/null +++ b/src/perception/rai_whatisee/src/rai_whatisee_node.cpp @@ -0,0 +1,170 @@ +// Copyright (C) 2024 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if defined(ROS_DISTRO_HUMBLE) +#include +#elif defined(ROS_DISTRO_JAZZY) +#include +#else +#include // Default to .hpp for future distributions +#endif + +#include +#include +#include +#include +#include +#include +#include +#include + +class WhatISeeNode : public rclcpp::Node +{ +public: + WhatISeeNode() : Node("rai_whatisee_node") + { + constexpr float observation_interval_seconds_default = 0.5; // Look only every X seconds + constexpr float image_similarity_threshold_default = 0.9f; // Below threshold are considered + const std::string camera_color_topic_default = "/camera/color/image_raw"; + this->declare_parameter("camera_color_topic", camera_color_topic_default); + this->declare_parameter("observation_interval_seconds", observation_interval_seconds_default); + this->declare_parameter("image_similarity_threshold", image_similarity_threshold_default); + + camera_color_topic_ = get_parameter("camera_color_topic").as_string(); + try { + rclcpp::expand_topic_or_service_name(camera_color_topic_, get_name(), get_namespace()); + } catch (const rclcpp::exceptions::InvalidTopicNameError & e) { + std::cerr << "Invalid topic name: " << e.what() << std::endl; + } + + image_similarity_threshold_ = get_parameter("image_similarity_threshold").as_double(); + if (image_similarity_threshold_ > 1.0f || image_similarity_threshold_ < 0.0f) { + RCLCPP_WARN(get_logger(), "Image similarity threshold should be between 1 and 0"); + image_similarity_threshold_ = std::clamp(image_similarity_threshold_, 0.0f, 1.0f); + } + + auto interval_seconds = get_parameter("observation_interval_seconds").as_double(); + if (interval_seconds < 0.0) { + RCLCPP_WARN(get_logger(), "Invalid observation interval parameter, setting to 0"); + interval_seconds = 0.0; + } + observation_interval_ = rclcpp::Duration::from_seconds(interval_seconds); + + camera_color_image_subscription_ = create_subscription( + camera_color_topic_, rclcpp::SensorDataQoS(), + std::bind(&WhatISeeNode::image_callback, this, std::placeholders::_1)); + + anything_new_srv_ = create_service( + "rai/whatisee/anything_new", + std::bind(&WhatISeeNode::anything_new, this, std::placeholders::_1, std::placeholders::_2)); + + whatisee_srv_ = create_service( + "rai/whatisee/get", + std::bind(&WhatISeeNode::what_i_see, this, std::placeholders::_1, std::placeholders::_2)); + + last_observation_timestamp_ = get_clock()->now(); + } + +private: + void anything_new( + [[maybe_unused]] const std::shared_ptr request, + const std::shared_ptr response) + { + std::lock_guard lock(mutex_); + if (!fresh_color_camera_image_) { + response->success = false; + return; + } + + if (!last_color_camera_image_) { + response->success = true; + return; + } + + response->success = + fresh_color_camera_image_->header.stamp != last_color_camera_image_->header.stamp; + } + + void what_i_see( + [[maybe_unused]] const std::shared_ptr request, + const std::shared_ptr response) + { + std::lock_guard lock(mutex_); + if (!fresh_color_camera_image_) { + response->observations.push_back("no image"); + return; + } + + response->observations.push_back("nothing to add"); + response->perception_source = camera_color_topic_; + response->image = *fresh_color_camera_image_; + last_color_camera_image_ = fresh_color_camera_image_; + } + + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + auto time_now = get_clock()->now(); + if (time_now - last_observation_timestamp_ > observation_interval_) { + if (is_image_novel(msg)) { + std::lock_guard lock(mutex_); + fresh_color_camera_image_ = msg; + } + last_observation_timestamp_ = time_now; + } + } + + // we compare to the last requested message, not to the recent message + bool is_image_novel([[maybe_unused]] const sensor_msgs::msg::Image::SharedPtr msg) + { + if (!last_color_camera_image_) { + return true; + } + cv::Mat new_img_opencv; + cv::Mat old_img_opencv; + { + std::lock_guard lock(mutex_); + try { + new_img_opencv = cv_bridge::toCvCopy(msg, "mono8")->image; + // TODO - save the old conversion + old_img_opencv = cv_bridge::toCvCopy(last_color_camera_image_, "mono8")->image; + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to convert image: %s", e.what()); + return false; + } + } + const cv::Scalar ssim = + cv::quality::QualitySSIM::compute(new_img_opencv, old_img_opencv, cv::noArray()); + return ssim[0] < image_similarity_threshold_; + } + + rclcpp::Subscription::SharedPtr camera_color_image_subscription_; + rclcpp::Service::SharedPtr whatisee_srv_; + rclcpp::Service::SharedPtr anything_new_srv_; + + rclcpp::Duration observation_interval_{0, 0}; // Zero is allowed, means no interval filtering + float image_similarity_threshold_; + std::string camera_color_topic_; + rclcpp::Time last_observation_timestamp_; + sensor_msgs::msg::Image::SharedPtr fresh_color_camera_image_; + sensor_msgs::msg::Image::SharedPtr last_color_camera_image_; + std::mutex mutex_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/rai_interfaces/CMakeLists.txt b/src/rai_interfaces/CMakeLists.txt index affa280c..5ef1a3fd 100644 --- a/src/rai_interfaces/CMakeLists.txt +++ b/src/rai_interfaces/CMakeLists.txt @@ -23,7 +23,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/RAIGroundingDino.srv" "action/Task.action" "action/TaskFeedback.action" - DEPENDENCIES std_msgs vision_msgs sensor_msgs + "srv/WhatISee.srv" + DEPENDENCIES std_msgs vision_msgs sensor_msgs geometry_msgs ) if(BUILD_TESTING) diff --git a/src/rai_interfaces/package.xml b/src/rai_interfaces/package.xml index e8cd6d6d..6627000a 100644 --- a/src/rai_interfaces/package.xml +++ b/src/rai_interfaces/package.xml @@ -2,13 +2,14 @@ rai_interfaces - 0.1.0 + 0.2.0 Interfaces for RAI communication Adam Dabrowski Kajetan Rachwał Apache-2.0 std_msgs + geometry_msgs vision_msgs sensor_msgs diff --git a/src/rai_interfaces/srv/WhatISee.srv b/src/rai_interfaces/srv/WhatISee.srv new file mode 100644 index 00000000..21b7175b --- /dev/null +++ b/src/rai_interfaces/srv/WhatISee.srv @@ -0,0 +1,23 @@ +# Copyright (C) 2024 Robotec.AI +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# + +# Request (empty) + +--- +# Response, timed with image timestamp +string[] observations +string perception_source +sensor_msgs/Image image +geometry_msgs/Pose pose