diff --git a/.clang-format b/.clang-format index d00892a..5ad3bba 100644 --- a/.clang-format +++ b/.clang-format @@ -1,72 +1,3 @@ -# Config based on: -# https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/humble/.clang-format - ---- -BasedOnStyle: Google -AccessModifierOffset: -2 -ConstructorInitializerIndentWidth: 2 -AlignAfterOpenBracket: AlwaysBreak -AlignEscapedNewlinesLeft: true -AlignTrailingComments: true -AllowAllArgumentsOnNextLine: false -AllowAllParametersOfDeclarationOnNextLine: false -PackConstructorInitializers: NextLine -AllowShortFunctionsOnASingleLine: None -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: true -BinPackArguments: false -BinPackParameters: false -BreakBeforeBinaryOperators: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: true +BasedOnStyle: Chromium +SortIncludes: false ColumnLimit: 120 -ConstructorInitializerAllOnOneLineOrOnePerLine: true -DerivePointerBinding: false -PointerBindsToType: true -ExperimentalAutoDetectBinPacking: false -IndentCaseLabels: true -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: None -ObjCSpaceBeforeProtocolList: true -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 60 -PenaltyBreakString: 1 -PenaltyBreakFirstLessLess: 1000 -PenaltyExcessCharacter: 1000 -PenaltyReturnTypeOnItsOwnLine: 90 -SpacesBeforeTrailingComments: 2 -Cpp11BracedListStyle: false -Standard: Auto -IndentWidth: 2 -TabWidth: 2 -UseTab: Never -IndentFunctionDeclarationAfterType: false -SpacesInParentheses: false -SpacesInAngles: false -SpaceInEmptyParentheses: false -SpacesInCStyleCastParentheses: false -SpacesInContainerLiterals: false -SpaceAfterControlStatementKeyword: true -SpaceBeforeAssignmentOperators: true -ContinuationIndentWidth: 4 -SortIncludes: true -SpaceAfterCStyleCast: false - -# Configure each individual brace in BraceWrapping -BreakBeforeBraces: Custom - -# Control of individual brace wrapping cases -BraceWrapping: - AfterCaseLabel: "false" - AfterClass: "false" - AfterControlStatement: Never - AfterEnum: "false" - AfterFunction: "false" - AfterNamespace: "false" - AfterStruct: "false" - AfterUnion: "false" - BeforeCatch: "false" - BeforeElse: "false" - IndentBraces: "false" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 7d08dcc..6a4f91a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -47,7 +47,7 @@ repos: - id: cmake-lint - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v22.1.1 + rev: v22.1.2 hooks: - id: clang-format types_or: [c++, proto] diff --git a/CHANGELOG.md b/CHANGELOG.md new file mode 100644 index 0000000..dc03a6c --- /dev/null +++ b/CHANGELOG.md @@ -0,0 +1,18 @@ +# Changelog + +All notable changes to the `scene_objects_manager` package will be documented in this file. + +The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.1.0/), +and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). + +## [Unreleased] + +### Added + +* [PR-5](https://github.com/AGH-CEAI/scene_objects_manager/pull/5) - Add a service to get the pose of marked objects in the scene. + +### Changed +### Deprecated +### Removed +### Fixed +### Security diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b0fdea..a34d068 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,32 +6,71 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() +set(HW_IF_INCLUDE_DEPENDS + cv_bridge + geometry_msgs + moveit_msgs + moveit_ros_planning_interface + OpenCV + rosidl_default_generators + std_msgs + sensor_msgs + tf2 + tf2_ros + tf2_geometry_msgs + yaml-cpp) + find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) find_package(rclcpp REQUIRED) -find_package(moveit_msgs REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) -find_package(std_msgs REQUIRED) -find_package(yaml-cpp REQUIRED) + +foreach(dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS}) + find_package(${dependency} REQUIRED) +endforeach() + include(FetchContent) FetchContent_Declare(argparse GIT_REPOSITORY https://github.com/p-ranav/argparse.git) FetchContent_MakeAvailable(argparse) -set(SOURCE_FILES src/args_parser.cpp src/scene_objects_manager.cpp - src/spawner.cpp src/yaml_parser.cpp) +set(SOURCE_FILES + src/args_parser.cpp src/scene_objects_manager.cpp src/spawner.cpp + src/yaml_parser.cpp src/object_pose_detector.cpp) + +rosidl_generate_interfaces(${PROJECT_NAME} "srv/DetectBlocksPoses.srv" + DEPENDENCIES geometry_msgs) + +add_executable(${PROJECT_NAME}_node ${SOURCE_FILES}) + +rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} + "rosidl_typesupport_cpp") + +target_link_libraries(${PROJECT_NAME}_node ${cpp_typesupport_target}) -add_executable(${PROJECT_NAME} ${SOURCE_FILES}) -ament_target_dependencies(${PROJECT_NAME} moveit_ros_planning_interface rclcpp - std_msgs) +ament_target_dependencies( + ${PROJECT_NAME}_node + ament_index_cpp + moveit_ros_planning_interface + rclcpp + std_msgs + rclcpp + sensor_msgs + geometry_msgs + cv_bridge + tf2 + tf2_ros + tf2_geometry_msgs) target_include_directories( - ${PROJECT_NAME} + ${PROJECT_NAME}_node PUBLIC "$" "$" ${argparse_SOURCE_DIR}/argparse/include) -target_link_libraries(${PROJECT_NAME} argparse yaml-cpp) +target_link_libraries(${PROJECT_NAME}_node argparse yaml-cpp + ${OpenCV_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) +install(TARGETS ${PROJECT_NAME}_node DESTINATION lib/${PROJECT_NAME}) +ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/README.md b/README.md index ad906d1..19f365e 100644 --- a/README.md +++ b/README.md @@ -17,5 +17,25 @@ Check the following messages definitions: | `shape_msgs/msg/SolidPrimitive.msg` | [link](https://github.com/ros2/common_interfaces/blob/humble/shape_msgs/msg/SolidPrimitive.msg) | +## Block detection service (based on ArUco markers) +This service detects blocks marked with ArUco markers using scene camera and returns their poses. + +### Service description +The `/detect_blocks_poses` service provides the poses of all detected blocks in the scene. +- Service name: `detect_blocks_poses` +- Service type: `scene_objects_manager/srv/DetectBlocksPoses` +- Response: `geometry_msgs/msg/PoseArray` + +The response contains a list of poses (`PoseArray`), where each pose corresponds to a detected block in the camera frame. + +### Example usage: +```bash +ros2 service call /detect_blocks_poses scene_objects_manager/srv/DetectBlocksPoses "{detect: true}" +``` + ## Libraries used - [argparse](https://github.com/p-ranav/argparse) (MIT License) + +## Tests +- `detect_all_markers.py` - is publishing static transform to the tf of detected blocks. +- `go_above_markers.py` - is moving the robot above the detected markers. diff --git a/include/scene_objects_manager/args_parser.hpp b/include/scene_objects_manager/args_parser.hpp index 11af868..c92d242 100644 --- a/include/scene_objects_manager/args_parser.hpp +++ b/include/scene_objects_manager/args_parser.hpp @@ -14,10 +14,9 @@ struct LaunchArguments { bool validate_launch_arguments(const LaunchArguments& args); -const LaunchArguments parse_args( - const std::string& program_name, - const std::string& program_version, - const std::vector& input_args); +const LaunchArguments parse_args(const std::string& program_name, + const std::string& program_version, + const std::vector& input_args); } // namespace sobjmanager #endif // SCENE_OBJECTS_MANAGER__ARGS_PARSER_HPP_ diff --git a/include/scene_objects_manager/object_pose_detector.hpp b/include/scene_objects_manager/object_pose_detector.hpp new file mode 100644 index 0000000..937c643 --- /dev/null +++ b/include/scene_objects_manager/object_pose_detector.hpp @@ -0,0 +1,73 @@ +#ifndef SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ +#define SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +using DetectBlocksPosesSrv = scene_objects_manager::srv::DetectBlocksPoses; + +namespace sobjmanager { + +class ObjectPoseDetectorNode : public rclcpp::Node { + public: + ObjectPoseDetectorNode(); + + private: + void image_cb(const sensor_msgs::msg::Image::SharedPtr msg); + + void on_detect(const std::shared_ptr, + std::shared_ptr res); + + void camera_info_cb(const sensor_msgs::msg::CameraInfo::SharedPtr msg); + + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Service::SharedPtr detect_blocks_srv_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + + std::mutex mtx_; + std::optional last_rgb_; + rclcpp::Time last_rgb_stamp_; + bool camera_info_received_; + float z_offset_of_block_; + + std::string image_topic_; + std::string cam_info_topic_; + std::string cam_frame_; + std::string output_frame_; + double aruco_size_; + + cv::Mat camera_matrix_; + cv::Mat dist_coeffs_; + + std::vector rvecs_; + std::vector tvecs_; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace sobjmanager +#endif // SCENE_OBJECTS_MANAGER__OBJECT_POSE_DETECTOR_HPP_ diff --git a/include/scene_objects_manager/scene_object.hpp b/include/scene_objects_manager/scene_object.hpp index d25e70e..621c9f4 100644 --- a/include/scene_objects_manager/scene_object.hpp +++ b/include/scene_objects_manager/scene_object.hpp @@ -25,8 +25,8 @@ struct SceneObject { Pose pose; static const inline std::map PRIMITIVE_STR_MAP = { - { "box", SolidPrimitive::BOX }, { "sphere", SolidPrimitive::SPHERE }, { "cylinder", SolidPrimitive::CYLINDER }, - { "cone", SolidPrimitive::CONE }, { "prism", SolidPrimitive::PRISM }, + {"box", SolidPrimitive::BOX}, {"sphere", SolidPrimitive::SPHERE}, {"cylinder", SolidPrimitive::CYLINDER}, + {"cone", SolidPrimitive::CONE}, {"prism", SolidPrimitive::PRISM}, }; static const inline std::map PRIMITIVE_UINT_MAP = reverse_map(PRIMITIVE_STR_MAP); }; diff --git a/include/scene_objects_manager/scene_objects_manager.hpp b/include/scene_objects_manager/scene_objects_manager.hpp index 0da7c0f..518399c 100644 --- a/include/scene_objects_manager/scene_objects_manager.hpp +++ b/include/scene_objects_manager/scene_objects_manager.hpp @@ -12,8 +12,8 @@ namespace sobjmanager { class SceneObjectsManager : public rclcpp::Node { -public: - SceneObjectsManager() : Node("SceneObjectsManager") {}; + public: + SceneObjectsManager() : Node("scene_objects_manager") {}; }; } // namespace sobjmanager diff --git a/include/scene_objects_manager/spawner.hpp b/include/scene_objects_manager/spawner.hpp index f8b3ec6..b5bbe7d 100644 --- a/include/scene_objects_manager/spawner.hpp +++ b/include/scene_objects_manager/spawner.hpp @@ -11,12 +11,12 @@ using CollisionObject = moveit_msgs::msg::CollisionObject; namespace sobjmanager { class Spawner { -public: + public: Spawner(std::string planning_frame_id); void spawn_object(const SceneObject& obj); -private: + private: static CollisionObject make_collision_object(const SceneObject& raw_obj); void spawn(const CollisionObject& object); diff --git a/package.xml b/package.xml index 8306827..b9ace0c 100644 --- a/package.xml +++ b/package.xml @@ -5,16 +5,28 @@ 0.0.0 Node for managing objects within MoveIt2 PlanningSceneInterface. Maciej Aleksandrowicz + Antoni Radomiński-Lasek Apache-2.0 ament_cmake + cv_bridge + geometry_msgs moveit_msgs moveit_ros_planning_interface rclcpp + sensor_msgs std_msgs + tf2 + tf2_geometry_msgs + tf2_ros yaml-cpp + rosidl_default_generators + rosidl_default_runtime + + rosidl_interface_packages + ament_lint_auto ament_lint_common diff --git a/src/args_parser.cpp b/src/args_parser.cpp index cc29256..eb38322 100644 --- a/src/args_parser.cpp +++ b/src/args_parser.cpp @@ -13,10 +13,9 @@ bool validate_launch_arguments(const LaunchArguments& args) { return true; } -const LaunchArguments parse_args( - const std::string& program_name, - const std::string& program_version, - const std::vector& input_args) { +const LaunchArguments parse_args(const std::string& program_name, + const std::string& program_version, + const std::vector& input_args) { argparse::ArgumentParser program(program_name, program_version, argparse::default_arguments::help); LaunchArguments args; diff --git a/src/object_pose_detector.cpp b/src/object_pose_detector.cpp new file mode 100644 index 0000000..3bf8390 --- /dev/null +++ b/src/object_pose_detector.cpp @@ -0,0 +1,140 @@ +#include "scene_objects_manager/object_pose_detector.hpp" + +namespace sobjmanager { + +ObjectPoseDetectorNode::ObjectPoseDetectorNode() : rclcpp::Node("object_pose_detector") { + aruco_size_ = this->declare_parameter("aruco_size", 0.02); + image_topic_ = this->declare_parameter("image_topic", "/cam_scene/rgb/image_raw"); + cam_info_topic_ = this->declare_parameter("cam_info_topic", "/cam_scene/rgb/camera_info"); + output_frame_ = this->declare_parameter("output_frame", "base_link"); + cam_frame_ = this->declare_parameter("cam_frame", "cam_scene_rgb_camera_optical_frame_cal"); + + camera_info_received_ = false; + z_offset_of_block_ = 0.095; + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + image_sub_ = this->create_subscription( + image_topic_, rclcpp::SensorDataQoS(), std::bind(&ObjectPoseDetectorNode::image_cb, this, std::placeholders::_1)); + + cam_info_sub_ = this->create_subscription( + cam_info_topic_, rclcpp::SensorDataQoS(), + std::bind(&ObjectPoseDetectorNode::camera_info_cb, this, std::placeholders::_1)); + + detect_blocks_srv_ = this->create_service( + "detect_blocks_poses", + std::bind(&ObjectPoseDetectorNode::on_detect, this, std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Detector node started. Subscriptionto: %s", image_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), "Service ready: /detect_blocks_poses"); +} + +void ObjectPoseDetectorNode::image_cb(const sensor_msgs::msg::Image::SharedPtr msg) { + cv_bridge::CvImageConstPtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvShare(msg, "bgr8"); + } catch (const cv_bridge::Exception& e) { + RCLCPP_WARN(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + { + std::lock_guard lock(mtx_); + last_rgb_ = cv_ptr->image.clone(); + last_rgb_stamp_ = msg->header.stamp; + } +} + +void ObjectPoseDetectorNode::on_detect(const std::shared_ptr, + std::shared_ptr res) { + if (!camera_info_received_) { + RCLCPP_INFO(this->get_logger(), "Camera info not received yet."); + return; + } + + cv::Mat img; + if (last_rgb_) { + std::lock_guard lock(mtx_); + img = last_rgb_.value().clone(); + } + + if (img.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed: no image in buffer to detect objects."); + return; + } + + res->poses.header.stamp = this->now(); + res->poses.header.frame_id = output_frame_; + res->poses.poses.clear(); + + std::vector marker_ids; + std::vector> marker_corners, rejected_candidates; + cv::Ptr parameters = cv::aruco::DetectorParameters::create(); + cv::Ptr dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); + cv::aruco::detectMarkers(img, dictionary, marker_corners, marker_ids, parameters, rejected_candidates); + + cv::aruco::estimatePoseSingleMarkers(marker_corners, aruco_size_, camera_matrix_, dist_coeffs_, rvecs_, tvecs_); + + cv::Mat R; + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + double k_angle_step = M_PI / 2.0; + tf2::Quaternion q_yaw_only; + + for (size_t i = 0; i < marker_ids.size(); i++) { + geometry_msgs::msg::PoseStamped pose_cam_; + pose_cam_.header.stamp = this->now(); + pose_cam_.header.frame_id = cam_frame_; + pose_cam_.pose.position.x = tvecs_[i][0]; + pose_cam_.pose.position.y = tvecs_[i][1]; + pose_cam_.pose.position.z = tvecs_[i][2]; + cv::Rodrigues(rvecs_[i], R); + tf2::Matrix3x3 tf3d(R.at(0, 0), R.at(0, 1), R.at(0, 2), R.at(1, 0), + R.at(1, 1), R.at(1, 2), R.at(2, 0), R.at(2, 1), + R.at(2, 2)); + + tf3d.getRPY(roll, pitch, yaw); + + yaw -= k_angle_step * std::round(yaw / k_angle_step) - k_angle_step; + + q_yaw_only.setRPY(0.0, 0.0, yaw); + q_yaw_only.normalize(); + pose_cam_.pose.orientation = tf2::toMsg(q_yaw_only); + + geometry_msgs::msg::PoseStamped pose_target; + try { + pose_target = tf_buffer_->transform(pose_cam_, output_frame_, tf2::durationFromSec(0.1)); + } catch (const tf2::TransformException& ex) { + RCLCPP_WARN(this->get_logger(), "TF transforms failed: %s", ex.what()); + continue; + } + + pose_target.pose.position.z = z_offset_of_block_; + + res->poses.poses.push_back(pose_target.pose); + } +} + +void ObjectPoseDetectorNode::camera_info_cb(const sensor_msgs::msg::CameraInfo::SharedPtr msg) { + if (camera_info_received_) + return; + + camera_matrix_ = cv::Mat(3, 3, CV_64F); + for (int row = 0; row < 3; row++) { + for (int col = 0; col < 3; col++) { + camera_matrix_.at(row, col) = msg->k[row * 3 + col]; + } + } + + dist_coeffs_ = cv::Mat(1, static_cast(msg->d.size()), CV_64F); + for (size_t i = 0; i < msg->d.size(); i++) { + dist_coeffs_.at(0, static_cast(i)) = msg->d[i]; + } + + camera_info_received_ = true; + + cam_info_sub_.reset(); +} + +} // namespace sobjmanager diff --git a/src/object_spawner.cpp b/src/object_spawner.cpp index 1579a3f..d62cb33 100644 --- a/src/object_spawner.cpp +++ b/src/object_spawner.cpp @@ -3,9 +3,8 @@ namespace sobjmanager { ObjectsSpawner::ObjectsSpawner(std::string planning_frame_id) - : _planning_frame_id(planning_frame_id) - , _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) { -} + : _planning_frame_id(planning_frame_id), + _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) {} void ObjectsSpawner::spawn_object(const SceneObject& obj) { spawn(make_collision_object(obj)); diff --git a/src/scene_objects_manager.cpp b/src/scene_objects_manager.cpp index 7c57eba..2676c64 100644 --- a/src/scene_objects_manager.cpp +++ b/src/scene_objects_manager.cpp @@ -1,6 +1,7 @@ #include "scene_objects_manager/scene_objects_manager.hpp" #include "scene_objects_manager/args_parser.hpp" +#include "scene_objects_manager/object_pose_detector.hpp" #include "scene_objects_manager/spawner.hpp" #include "scene_objects_manager/yaml_parser.hpp" @@ -11,26 +12,27 @@ int main(int argc, char* argv[]) { std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); return parse_args("SceneObjectsSpawner", "0.0.0", args); }(); - auto const node = std::make_shared(); + + auto manager_node = std::make_shared(); + auto detector_node = std::make_shared(); auto scene_objs = load_scene_objects_from_yaml(launch_args.cfg_path); - RCLCPP_INFO( - node->get_logger(), - "Loaded %d object(s) from the configuration file.", - static_cast(scene_objs.size())); + RCLCPP_INFO(manager_node->get_logger(), "Loaded %d object(s) from the configuration file.", + static_cast(scene_objs.size())); auto spawner = Spawner(launch_args.planning_frame_id); for (const auto& obj : scene_objs) { spawner.spawn_object(obj); - RCLCPP_INFO( - node->get_logger(), - "Spawned new scene object \"%s\" (unique_id: %s).", - obj.pretty_name.c_str(), - obj.unique_id.c_str()); + RCLCPP_INFO(manager_node->get_logger(), "Spawned new scene object \"%s\" (unique_id: %s).", obj.pretty_name.c_str(), + obj.unique_id.c_str()); } - RCLCPP_INFO(node->get_logger(), "Spawned all objects."); + RCLCPP_INFO(manager_node->get_logger(), "Spawned all objects."); + + rclcpp::executors::MultiThreadedExecutor executor_; + executor_.add_node(manager_node); + executor_.add_node(detector_node); + executor_.spin(); - rclcpp::spin(node); rclcpp::shutdown(); return 0; } diff --git a/src/spawner.cpp b/src/spawner.cpp index 04992f8..f640ff1 100644 --- a/src/spawner.cpp +++ b/src/spawner.cpp @@ -3,9 +3,8 @@ namespace sobjmanager { Spawner::Spawner(std::string planning_frame_id) - : _planning_frame_id(planning_frame_id) - , _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) { -} + : _planning_frame_id(planning_frame_id), + _planning_scene_interface(moveit::planning_interface::PlanningSceneInterface()) {} void Spawner::spawn_object(const SceneObject& obj) { spawn(make_collision_object(obj)); diff --git a/srv/DetectBlocksPoses.srv b/srv/DetectBlocksPoses.srv new file mode 100644 index 0000000..41666ff --- /dev/null +++ b/srv/DetectBlocksPoses.srv @@ -0,0 +1,3 @@ +--- +# Response +geometry_msgs/PoseArray poses diff --git a/test/detect_all_markers.py b/test/detect_all_markers.py new file mode 100644 index 0000000..bbd1453 --- /dev/null +++ b/test/detect_all_markers.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python3 +import rclpy + +from geometry_msgs.msg import PoseStamped, TransformStamped +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +from scene_objects_manager.srv import DetectBlocksPoses +from aegis_director.robot_director import RobotDirector + + +def main(): + rclpy.init() + + director = RobotDirector(synchronous=True) + + service_name = "/detect_blocks_poses" + target_frame_fallback = "base_link" + z_offset = 0.0 + + cli = director.node.create_client(DetectBlocksPoses, service_name) + static_broadcaster = StaticTransformBroadcaster(director.node) + + director.node.get_logger().info(f"Waiting for service {service_name} ...") + if not cli.wait_for_service(timeout_sec=10.0): + director.node.get_logger().error(f"Service {service_name} not available.") + director.node.destroy_node() + rclpy.shutdown() + return + + req = DetectBlocksPoses.Request() + future = cli.call_async(req) + rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) + + resp = future.result() + if resp is None: + director.node.get_logger().error("Service call failed / timed out.") + director.node.destroy_node() + rclpy.shutdown() + return + + pose_array = resp.poses + frame_id = pose_array.header.frame_id or target_frame_fallback + + director.node.get_logger().info( + f"Got {len(pose_array.poses)} pose(s) in frame '{frame_id}'." + ) + + targets: list[PoseStamped] = [] + static_transforms: list[TransformStamped] = [] + + now = director.node.get_clock().now().to_msg() + + for i, p in enumerate(pose_array.poses): + target = PoseStamped() + target.header.stamp = now + target.header.frame_id = frame_id + target.pose = p + target.pose.position.z += z_offset + + targets.append(target) + + director.node.get_logger().info( + f"[{i + 1}] above: x={target.pose.position.x:.3f}, " + f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" + ) + + tf_msg = TransformStamped() + tf_msg.header.stamp = now + tf_msg.header.frame_id = frame_id + tf_msg.child_frame_id = f"detected_block_{i + 1}" + + tf_msg.transform.translation.x = target.pose.position.x + tf_msg.transform.translation.y = target.pose.position.y + tf_msg.transform.translation.z = target.pose.position.z + + tf_msg.transform.rotation = target.pose.orientation + + static_transforms.append(tf_msg) + + if static_transforms: + static_broadcaster.sendTransform(static_transforms) + director.node.get_logger().info( + f"Published {len(static_transforms)} static TF frame(s)." + ) + + director.node.get_logger().info("Done with all targets.") + + rclpy.spin_once(director.node, timeout_sec=0.5) + + director.node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/test/go_above_markers.py b/test/go_above_markers.py new file mode 100644 index 0000000..279812b --- /dev/null +++ b/test/go_above_markers.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +import time + +import rclpy +from geometry_msgs.msg import PoseStamped + +from scene_objects_manager.srv import DetectBlocksPoses +from aegis_director.robot_director import RobotDirector + + +def main(): + rclpy.init() + + director = RobotDirector(synchronous=True) + + NUM_OF_ITER = 1 + + for i in range(NUM_OF_ITER): + director.joint_move( + joint_positions={ + "shoulder_pan_joint": 0.0, + "shoulder_lift_joint": -2.09, + "elbow_joint": 2.09, + "wrist_1_joint": -1.57, + "wrist_2_joint": -1.57, + "wrist_3_joint": 0.0, + }, + max_vel=0.1, + max_accel=0.1, + ) + + time.sleep(2.0) + + service_name = "/detect_blocks_poses" + target_frame_fallback = "base_link" + z_offset = 0.005 + wait_at_target_s = 3.0 + + cli = director.node.create_client(DetectBlocksPoses, service_name) + director.node.get_logger().info(f"Waiting for service {service_name} ...") + if not cli.wait_for_service(timeout_sec=10.0): + director.node.get_logger().error(f"Service {service_name} not available.") + director.node.destroy_node() + rclpy.shutdown() + return + + req = DetectBlocksPoses.Request() + future = cli.call_async(req) + rclpy.spin_until_future_complete(director.node, future, timeout_sec=10.0) + + resp = future.result() + if resp is None: + director.node.get_logger().error("Service call failed / timed out.") + director.node.destroy_node() + rclpy.shutdown() + return + + pose_array = resp.poses + frame_id = pose_array.header.frame_id or target_frame_fallback + + director.node.get_logger().info( + f"Got {len(pose_array.poses)} pose(s) in frame '{frame_id}'." + ) + + targets: list[PoseStamped] = [] + for i, p in enumerate(pose_array.poses): + target = PoseStamped() + target.header.stamp = director.node.get_clock().now().to_msg() + target.header.frame_id = frame_id + target.pose = p + target.pose.position.z += z_offset + + targets.append(target) + + director.node.get_logger().info( + f"[{i + 1}] above: x={target.pose.position.x:.3f}, " + f"y={target.pose.position.y:.3f}, z={target.pose.position.z:.3f}" + ) + + for i, t in enumerate(targets): + director.node.get_logger().info( + f"Moving to target {i + 1}/{len(targets)} ..." + ) + director.pose_move( + pose=t, + cartesian=False, + max_vel=0.1, + max_accel=0.1, + ) + director.node.get_logger().info( + f"Reached target {i + 1}. Waiting {wait_at_target_s:.1f}s ..." + ) + time.sleep(wait_at_target_s) + + director.node.get_logger().info("Done with all targets.") + director.node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()