diff --git a/src/panda/README.md b/src/panda/README.md index 8871b5c..a210c9a 100644 --- a/src/panda/README.md +++ b/src/panda/README.md @@ -46,7 +46,7 @@ ros2 launch panda panda_pick_and_place_cubes.launch.py ros2 launch panda panda_pick_and_place_cubes.launch.py cube_count:=5 # Restricted corridor: narrow passage obstacle-avoidance demo -ros2 launch panda panda_restricted_corridor.launch.py +ros2 launch panda panda_multi_object_sequence.launch.py # Restricted pillars: pillar field obstacle-avoidance demo ros2 launch panda panda_restricted_pillars.launch.py diff --git a/src/panda/launch/panda_restricted_corridor.launch.py b/src/panda/launch/panda_multi_object_sequence.launch.py similarity index 54% rename from src/panda/launch/panda_restricted_corridor.launch.py rename to src/panda/launch/panda_multi_object_sequence.launch.py index 8d6687b..94cbd3f 100644 --- a/src/panda/launch/panda_restricted_corridor.launch.py +++ b/src/panda/launch/panda_multi_object_sequence.launch.py @@ -16,9 +16,19 @@ def generate_launch_description() -> LaunchDescription: ), launch_arguments={ 'world_file': PathJoinSubstitution( - [FindPackageShare('panda'), 'worlds', 'restricted_corridor_env.sdf'] + [FindPackageShare('panda'), 'worlds', 'multi_object_sequential_env.sdf'] ), 'bridge_external_camera': 'false', + # External RGB-D camera positioned in front of the robot and table, + # elevated and angled down so it sees the full workspace: all three + # target objects, the corridor walls/barriers, and the Panda arm. + 'spawn_external_rgbd_camera': 'true', + 'external_camera_x': '1.10', + 'external_camera_y': '1.80', + 'external_camera_z': '2.10', + 'external_camera_roll': '0.0', + 'external_camera_pitch': '0.70', + 'external_camera_yaw': '3.14159', }.items(), ) ] diff --git a/src/panda/launch/panda_vla_pipeline.launch.py b/src/panda/launch/panda_vla_pipeline.launch.py new file mode 100644 index 0000000..d16584d --- /dev/null +++ b/src/panda/launch/panda_vla_pipeline.launch.py @@ -0,0 +1,61 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + """VLA sort-and-place environment. + + Table layout (top-down, robot at x=0 y=1.70 facing +x): + + obj_sphere (red ball) – (0.35, 1.72) + obj_puck (orange disc) – (0.45, 1.65) + obj_brick (green block) – (0.30, 1.90) + obj_cylinder(blue can) – (0.42, 1.88) + + basket_red – (0.55, 1.62) ← place red objects here + basket_blue – (0.55, 1.80) ← place blue objects here + basket_green – (0.55, 1.98) ← place green objects here + + External RGB-D camera: x=1.10, y=1.80, z=2.10, pitch=0.70, yaw=π + → positioned directly in front of the arm+table, elevated and angled + down so it captures all objects, all baskets, and the Panda end-effector. + + Wrist-eye camera: built into Panda URDF → /wrist_eye/image_raw + /wrist_eye/depth/image_raw + """ + return LaunchDescription( + [ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [FindPackageShare('panda'), 'launch', 'panda_pick_and_place.launch.py'] + ) + ), + launch_arguments={ + 'world_file': PathJoinSubstitution( + [FindPackageShare('panda'), 'worlds', 'vla_pipeline_env.sdf'] + ), + # Cube spawner disabled – objects are embedded in the world SDF. + 'cube_count': '0', + # Wrist camera is always bridged by panda_pick_and_place. + # External in-world camera (rgbd_camera topic) is not used here. + 'bridge_external_camera': 'false', + # Spawn a dedicated front-view RGB-D camera at run-time so + # its pose can be tuned without editing the world SDF. + 'spawn_external_rgbd_camera': 'true', + # Camera sits in front of the robot and table, looking back + # at roughly 40° downward (pitch=0.70 rad) to see the full + # table surface including objects and baskets. + 'external_camera_x': '1.10', + 'external_camera_y': '1.80', + 'external_camera_z': '2.10', + 'external_camera_roll': '0.0', + 'external_camera_pitch': '0.70', + 'external_camera_yaw': '3.14159', + }.items(), + ) + ] + ) diff --git a/src/panda/worlds/multi_object_sequential_env.sdf b/src/panda/worlds/multi_object_sequential_env.sdf new file mode 100644 index 0000000..9db2a01 --- /dev/null +++ b/src/panda/worlds/multi_object_sequential_env.sdf @@ -0,0 +1,303 @@ + + + + + 0.001 + 1.0 + 1000 + + + + + + ogre2 + + + + + 0.45 0.45 0.45 1 + 0.70 0.72 0.74 1 + true + + + 0 0 -9.8 + + + model://ground_plane + + + + model://sun + + + + model://table + 0.270759 1.801050 0 0 0 0 + + + + + + + true + 0.44 1.60 1.085 0 0 0 + + + + 0.48 0.03 0.14 + + + + + 0.48 0.03 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + true + 0.44 2.00 1.085 0 0 0 + + + + 0.48 0.03 0.14 + + + + + 0.48 0.03 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + + true + + 0.37 1.665 1.085 0 0 0 + + + + 0.03 0.13 0.14 + + + + + 0.03 0.13 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + true + + 0.37 1.935 1.085 0 0 0 + + + + 0.03 0.13 0.14 + + + + + 0.03 0.13 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + + true + 0.52 1.665 1.085 0 0 0 + + + + 0.03 0.13 0.14 + + + + + 0.03 0.13 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + true + 0.52 1.935 1.085 0 0 0 + + + + 0.03 0.13 0.14 + + + + + 0.03 0.13 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + + true + 0.68 1.800 1.085 0 0 0 + + + + 0.03 0.40 0.14 + + + + + 0.03 0.40 0.14 + + + 1 1 0 1 + 1 1 0 1 + + + + + + + + + + + false + 0.44 1.80 1.040 0 0 0 + + + 0.12 + + 0.000050.00.0 + 0.000050.0 + 0.00004 + + + + + 0.0250.05 + + + + + 0.0250.05 + + + 1 0 0 1 + 1 0 0 1 + 0.05 0.05 0.05 1 + + + + + + + + false + 0.60 1.80 1.040 0 0 0 + + + 0.12 + + 0.000030.00.0 + 0.000080.0 + 0.00008 + + + + + 0.08 0.04 0.04 + + + + + 0.08 0.04 0.04 + + + 0 0 1 1 + 0 0 1 1 + 0.05 0.05 0.05 1 + + + + + + diff --git a/src/panda/worlds/vla_pipeline_env.sdf b/src/panda/worlds/vla_pipeline_env.sdf new file mode 100644 index 0000000..e235e14 --- /dev/null +++ b/src/panda/worlds/vla_pipeline_env.sdf @@ -0,0 +1,304 @@ + + + + + 0.001 + 1.0 + 1000 + + + + + + ogre2 + + + + + 0.50 0.50 0.50 1 + 0.68 0.72 0.76 1 + true + + + 0 0 -9.8 + + + model://ground_plane + + + + model://sun + + + + + + + model://table + 0.270759 1.801050 0 0 0 0 + + + + + + + false + 0.350 1.720 1.040 0 0 0 + + + 0.08 + + 0.0000200.00.0 + 0.0000200.0 + 0.000020 + + + + + 0.025 + + + + + 0.025 + + + 0.90 0.15 0.15 1 + 0.90 0.15 0.15 1 + 0.20 0.05 0.05 1 + + + + + + + + false + 0.420 1.880 1.040 0 0 0 + + + 0.10 + + 0.0000180.00.0 + 0.0000180.0 + 0.000012 + + + + + 0.0220.055 + + + + + 0.0220.055 + + + 0.20 0.40 0.92 1 + 0.20 0.40 0.92 1 + 0.05 0.08 0.20 1 + + + + + + + + false + + 0.300 1.900 1.042 0 0 0 + + + 0.10 + + 0.0000120.00.0 + 0.0000300.0 + 0.000030 + + + + + 0.034 0.080 0.034 + + + + + 0.034 0.080 0.034 + + + 0.15 0.78 0.25 1 + 0.15 0.78 0.25 1 + 0.05 0.15 0.05 1 + + + + + + + + false + + 0.450 1.650 1.038 0 0 0 + + + 0.08 + + 0.0000150.00.0 + 0.0000150.0 + 0.000022 + + + + + 0.0300.026 + + + + + 0.0300.026 + + + 0.95 0.55 0.10 1 + 0.95 0.55 0.10 1 + 0.15 0.08 0.02 1 + + + + + + + + + + true + 0.550 1.620 1.015 0 0 0 + + + 0 0 0 0 0 0 + 0.160 0.160 0.010 + 0 0 0 0 0 0 + 0.160 0.160 0.010 + 0.85 0.15 0.15 10.85 0.15 0.15 1 + + 0 -0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0 -0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0.85 0.15 0.15 10.85 0.15 0.15 1 + + 0 0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0 0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0.85 0.15 0.15 10.85 0.15 0.15 1 + + -0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + -0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.85 0.15 0.15 10.85 0.15 0.15 1 + + 0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.85 0.15 0.15 10.85 0.15 0.15 1 + + + + + + true + 0.550 1.800 1.015 0 0 0 + + 0 0 0 0 0 0 + 0.160 0.160 0.010 + 0 0 0 0 0 0 + 0.160 0.160 0.010 + 0.20 0.40 0.92 10.20 0.40 0.92 1 + 0 -0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0 -0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0.20 0.40 0.92 10.20 0.40 0.92 1 + 0 0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0 0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0.20 0.40 0.92 10.20 0.40 0.92 1 + -0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + -0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.20 0.40 0.92 10.20 0.40 0.92 1 + 0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.20 0.40 0.92 10.20 0.40 0.92 1 + + + + + + true + 0.550 1.980 1.015 0 0 0 + + 0 0 0 0 0 0 + 0.160 0.160 0.010 + 0 0 0 0 0 0 + 0.160 0.160 0.010 + 0.15 0.78 0.25 10.15 0.78 0.25 1 + 0 -0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0 -0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0.15 0.78 0.25 10.15 0.78 0.25 1 + 0 0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0 0.075 0.035 0 0 0 + 0.160 0.010 0.060 + 0.15 0.78 0.25 10.15 0.78 0.25 1 + -0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + -0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.15 0.78 0.25 10.15 0.78 0.25 1 + 0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.075 0 0.035 0 0 0 + 0.010 0.160 0.060 + 0.15 0.78 0.25 10.15 0.78 0.25 1 + + + + + diff --git a/src/pick_place/CMakeLists.txt b/src/pick_place/CMakeLists.txt index 523a180..7d72f07 100644 --- a/src/pick_place/CMakeLists.txt +++ b/src/pick_place/CMakeLists.txt @@ -57,6 +57,13 @@ add_executable(pick_place_executor_node ) ament_target_dependencies(pick_place_executor_node ${dependencies}) +# Build pick_place_multi_executor_node executable +add_executable(pick_place_multi_executor_node + src/pick_place_multi_executor.cpp + src/pick_place_multi_executor_main.cpp +) +ament_target_dependencies(pick_place_multi_executor_node ${dependencies}) + # Build object_detector_pcl_node executable add_executable(object_detector_pcl_node src/object_detector_pcl.cpp @@ -70,6 +77,7 @@ install(TARGETS object_detector_node object_detector_pcl_node pick_place_executor_node + pick_place_multi_executor_node DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/pick_place/README.md b/src/pick_place/README.md index 5978eba..5d23843 100644 --- a/src/pick_place/README.md +++ b/src/pick_place/README.md @@ -18,4 +18,21 @@ ros2 run tf2_ros static_transform_publisher 0 0 0 0 0 0 panda_wrist_eye_link Pan ros2 run pick_place pick_place_executor_node --ros-args --params-file ros2_ws/src/dev_robotic_arm_engineering/src/pick_place/config/params.yaml +``` + +### For multiple objects + +```bash +# env +ros2 launch panda panda_multi_object_sequence.launch.py + +# finding objects and publishing their poses +ros2 launch cube_segmentation segmentation.launch.py \ + use_sim_time:=true \ + publish_header_alias_tf:=true \ + publish_optical_tf:=true + +# pick and place executor +ros2 launch pick_place pick_place_multi_object.launch.py + ``` diff --git a/src/pick_place/config/params.yaml b/src/pick_place/config/params.yaml index 43f10b4..b724ed2 100644 --- a/src/pick_place/config/params.yaml +++ b/src/pick_place/config/params.yaml @@ -101,3 +101,58 @@ detection_stability_count: 5 # frames before accepting detection detection_stability_dist: 0.02 # metres + + # Multi-object static-scene executor parameters + pick_place_multi_executor: + object_pose_topic: "/segmentation/object_poses" + arm_group_name: panda_arm + end_effector_link: panda_hand_tcp + hand_group_name: hand + home_named_target: ready + max_objects: 3 + + execute_enabled: true + execute_pick_only: false + continue_on_failure: true + + approach_offset_z: 0.1 + post_grasp_lift_z: 0.18 # metres; extra lift after grasp before transfer to place + cartesian_step: 0.01 + min_cartesian_fraction: 0.9 + enforce_min_z: true + min_allowed_z: 0.0 + + use_current_ee_orientation_for_pick: false + use_fixed_pick_orientation: true + pick_pose: + qx: 1.0 + qy: 0.0 + qz: 0.0 + qw: 0.0 + + # Place targets consumed by index in PoseArray order. + place_poses_x: [-0.059, -0.249, -0.335] + place_poses_y: [0.424, 0.424, 0.094] + place_poses_z: [0.20, 0.20, 0.20] + place_poses_qx: [1.0, 1.0, 1.0] + place_poses_qy: [0.0, 0.0, 0.0] + place_poses_qz: [0.0, 0.0, 0.0] + place_poses_qw: [0.0, 0.0, 0.0] + + # Static obstacle import from multi_object_sequential_env.sdf. + # Default offsets convert world coordinates to panda_link0 for the + # default spawn pose in panda_pick_and_place.launch.py. + publish_static_env_obstacles: true + static_env_sdf_path: "src/dev_robotic_arm_engineering/src/panda/worlds/multi_object_sequential_env.sdf" + static_env_offset_x: 0.0 + static_env_offset_y: -1.70 + static_env_offset_z: -1.0 + static_env_offset_roll: 0.0 + static_env_offset_pitch: 0.0 + static_env_offset_yaw: 0.0 + + table_proxy_enabled: true + table_proxy_size_x: 1.50 + table_proxy_size_y: 0.80 + table_proxy_size_z: 0.08 + table_proxy_center_z_offset: 1.0 diff --git a/src/pick_place/include/pick_place/pick_place_multi_executor.hpp b/src/pick_place/include/pick_place/pick_place_multi_executor.hpp new file mode 100644 index 0000000..38c9486 --- /dev/null +++ b/src/pick_place/include/pick_place/pick_place_multi_executor.hpp @@ -0,0 +1,107 @@ +#ifndef PICK_PLACE__PICK_PLACE_MULTI_EXECUTOR_HPP_ +#define PICK_PLACE__PICK_PLACE_MULTI_EXECUTOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pick_place { + + class PickPlaceMultiExecutor : public rclcpp::Node { + public: + explicit PickPlaceMultiExecutor(rclcpp::NodeOptions const& options = rclcpp::NodeOptions()); + + private: + enum class State { IDLE, WAITING_FOR_SNAPSHOT, RUNNING, COMPLETED }; + + void objectPosesCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg); + void rerunServiceCallback(const std::shared_ptr request, + std::shared_ptr response); + bool executeSequence(); + bool runPlanningSequenceForObject(geometry_msgs::msg::PoseStamped const& object_pose_base, + geometry_msgs::msg::PoseStamped const& place_pose_base, + size_t object_index); + bool loadStaticObstaclesFromSdf(); + bool transformPoseToBase(geometry_msgs::msg::PoseStamped const& input, + geometry_msgs::msg::PoseStamped& output) const; + + geometry_msgs::msg::PoseStamped computePreGraspPose(geometry_msgs::msg::PoseStamped const& object_pose_base) const; + bool planToPoseTarget(geometry_msgs::msg::PoseStamped const& target_pose, + std::string const& stage_name, + bool execute_after_plan); + bool planCartesianMove(geometry_msgs::msg::Pose const& target_pose, + std::string const& stage_name, + bool execute_after_plan); + bool planToNamedTarget(std::string const& target_name, std::string const& stage_name, bool execute_after_plan); + bool planGripperAction(std::string const& action_name, bool execute_action); + bool executeArmPlan(moveit::planning_interface::MoveGroupInterface::Plan const& plan, + std::string const& stage_name); + bool executeHandNamedTarget(std::string const& target_name, std::string const& stage_name); + void setState(State new_state); + char const* stateToString(State state) const; + + rclcpp::Subscription::SharedPtr object_poses_sub_; + rclcpp::Service::SharedPtr rerun_service_; + + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + std::shared_ptr arm_move_group_; + std::shared_ptr hand_move_group_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; + + std::vector object_poses_snapshot_; + std::vector place_poses_; + + std::string object_pose_topic_; + std::string base_frame_; + std::string arm_group_name_; + std::string end_effector_link_; + std::string hand_group_name_; + std::string home_named_target_; + + int64_t max_objects_{3}; + double approach_offset_z_{0.1}; + double post_grasp_lift_z_{0.1}; + double cartesian_step_{0.01}; + double min_cartesian_fraction_{0.9}; + double min_allowed_z_{0.0}; + bool enforce_min_z_{true}; + bool use_current_ee_orientation_for_pick_{false}; + bool use_fixed_pick_orientation_{true}; + geometry_msgs::msg::Quaternion fixed_pick_orientation_; + bool execute_enabled_{false}; + bool execute_pick_only_{false}; + bool continue_on_failure_{true}; + + bool publish_static_env_obstacles_{true}; + std::string static_env_sdf_path_; + double static_env_offset_x_{0.0}; + double static_env_offset_y_{-1.70}; + double static_env_offset_z_{-1.0}; + double static_env_offset_roll_{0.0}; + double static_env_offset_pitch_{0.0}; + double static_env_offset_yaw_{0.0}; + bool table_proxy_enabled_{true}; + double table_proxy_size_x_{1.50}; + double table_proxy_size_y_{0.80}; + double table_proxy_size_z_{0.08}; + double table_proxy_center_z_offset_{1.0}; + + bool snapshot_locked_{false}; + bool sequence_completed_once_{false}; + bool planning_busy_{false}; + State state_{State::IDLE}; + }; + +} // namespace pick_place + +#endif // PICK_PLACE__PICK_PLACE_MULTI_EXECUTOR_HPP_ diff --git a/src/pick_place/launch/pick_place_multi_object.launch.py b/src/pick_place/launch/pick_place_multi_object.launch.py new file mode 100644 index 0000000..29e85d7 --- /dev/null +++ b/src/pick_place/launch/pick_place_multi_object.launch.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description() -> LaunchDescription: + params_file_arg = DeclareLaunchArgument( + "params_file", + default_value=PathJoinSubstitution( + [FindPackageShare("pick_place"), "config", "params.yaml"] + ), + description="Path to pick_place params yaml", + ) + + multi_executor_node = Node( + package="pick_place", + executable="pick_place_multi_executor_node", + name="pick_place_multi_executor", + output="screen", + parameters=[LaunchConfiguration("params_file")], + ) + + return LaunchDescription([params_file_arg, multi_executor_node]) diff --git a/src/pick_place/src/pick_place_multi_executor.cpp b/src/pick_place/src/pick_place_multi_executor.cpp new file mode 100644 index 0000000..e4ba8f8 --- /dev/null +++ b/src/pick_place/src/pick_place_multi_executor.cpp @@ -0,0 +1,914 @@ +#include "pick_place/pick_place_multi_executor.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace pick_place { + + namespace { + + struct StaticObstacleSpec { + std::string name; + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Vector3 size; + }; + + std::string trim(std::string value) { + auto is_space = [](unsigned char c) { return std::isspace(c) != 0; }; + while (!value.empty() && is_space(static_cast(value.front()))) { + value.erase(value.begin()); + } + while (!value.empty() && is_space(static_cast(value.back()))) { + value.pop_back(); + } + return value; + } + + bool startsWith(std::string const& value, std::string const& prefix) { + return value.size() >= prefix.size() && std::equal(prefix.begin(), prefix.end(), value.begin()); + } + + std::vector parseDoubles(std::string const& text) { + std::vector values; + std::istringstream stream(text); + double v = 0.0; + while (stream >> v) { + values.push_back(v); + } + return values; + } + + std::string chooseArmGroupName(std::vector const& available_groups, + std::string const& requested_group) { + auto has_group = [&](std::string const& name) { + return std::find(available_groups.begin(), available_groups.end(), name) != available_groups.end(); + }; + + if (has_group(requested_group)) { + return requested_group; + } + if (has_group("panda_arm")) { + return "panda_arm"; + } + if (has_group("panda_arm_hand")) { + return "panda_arm_hand"; + } + if (has_group("arm")) { + return "arm"; + } + for (auto const& name : available_groups) { + if (name != "hand") { + return name; + } + } + return {}; + } + + geometry_msgs::msg::Pose applyEnvOffset(geometry_msgs::msg::Pose const& env_pose, + double tx, + double ty, + double tz, + double roll, + double pitch, + double yaw) { + tf2::Quaternion q_env( + env_pose.orientation.x, env_pose.orientation.y, env_pose.orientation.z, env_pose.orientation.w); + tf2::Vector3 p_env(env_pose.position.x, env_pose.position.y, env_pose.position.z); + + tf2::Transform t_env(q_env, p_env); + tf2::Quaternion q_offset; + q_offset.setRPY(roll, pitch, yaw); + tf2::Transform t_offset(q_offset, tf2::Vector3(tx, ty, tz)); + + tf2::Transform t_base = t_offset * t_env; + geometry_msgs::msg::Pose out; + out.position.x = t_base.getOrigin().x(); + out.position.y = t_base.getOrigin().y(); + out.position.z = t_base.getOrigin().z(); + out.orientation = tf2::toMsg(t_base.getRotation()); + return out; + } + + std::vector parseStaticObstaclesFromSdf(std::string const& sdf_text, + bool table_proxy_enabled, + double table_proxy_size_x, + double table_proxy_size_y, + double table_proxy_size_z, + double table_proxy_center_z_offset, + double static_env_offset_x, + double static_env_offset_y, + double static_env_offset_z, + double static_env_offset_roll, + double static_env_offset_pitch, + double static_env_offset_yaw) { + std::vector specs; + std::unordered_set excluded_names = {"target_cube", "target_cylinder", "target_cuboid"}; + + std::regex model_re("([\\s\\S]*?)"); + std::regex pose_re("([^<]+)"); + std::regex static_re("\\s*true\\s*"); + std::regex box_size_re("\\s*([^<]+)\\s*"); + + auto model_begin = std::sregex_iterator(sdf_text.begin(), sdf_text.end(), model_re); + auto model_end = std::sregex_iterator(); + + for (auto it = model_begin; it != model_end; ++it) { + std::string model_name = (*it)[1].str(); + std::string model_body = (*it)[2].str(); + if (excluded_names.count(model_name) > 0 || startsWith(model_name, "target_")) { + continue; + } + + if (!std::regex_search(model_body, static_re)) { + continue; + } + + std::smatch pose_match; + if (!std::regex_search(model_body, pose_match, pose_re)) { + continue; + } + auto pose_values = parseDoubles(trim(pose_match[1].str())); + if (pose_values.size() < 6) { + continue; + } + + std::smatch box_match; + if (!std::regex_search(model_body, box_match, box_size_re)) { + continue; + } + auto size_values = parseDoubles(trim(box_match[1].str())); + if (size_values.size() < 3) { + continue; + } + + geometry_msgs::msg::Pose env_pose; + env_pose.position.x = pose_values[0]; + env_pose.position.y = pose_values[1]; + env_pose.position.z = pose_values[2]; + tf2::Quaternion q_env; + q_env.setRPY(pose_values[3], pose_values[4], pose_values[5]); + env_pose.orientation = tf2::toMsg(q_env); + + StaticObstacleSpec spec; + spec.name = model_name; + spec.pose = applyEnvOffset(env_pose, + static_env_offset_x, + static_env_offset_y, + static_env_offset_z, + static_env_offset_roll, + static_env_offset_pitch, + static_env_offset_yaw); + spec.size.x = size_values[0]; + spec.size.y = size_values[1]; + spec.size.z = size_values[2]; + specs.push_back(spec); + } + + if (table_proxy_enabled) { + std::regex include_re("([\\s\\S]*?)"); + std::regex table_uri_re("\\s*model://table\\s*"); + std::regex include_pose_re("([^<]+)"); + auto include_begin = std::sregex_iterator(sdf_text.begin(), sdf_text.end(), include_re); + auto include_end = std::sregex_iterator(); + + for (auto it = include_begin; it != include_end; ++it) { + std::string include_body = (*it)[1].str(); + if (!std::regex_search(include_body, table_uri_re)) { + continue; + } + + geometry_msgs::msg::Pose env_pose; + env_pose.orientation.w = 1.0; + + std::smatch pose_match; + if (std::regex_search(include_body, pose_match, include_pose_re)) { + auto pose_values = parseDoubles(trim(pose_match[1].str())); + if (pose_values.size() >= 6) { + env_pose.position.x = pose_values[0]; + env_pose.position.y = pose_values[1]; + env_pose.position.z = pose_values[2] + table_proxy_center_z_offset; + tf2::Quaternion q_table; + q_table.setRPY(pose_values[3], pose_values[4], pose_values[5]); + env_pose.orientation = tf2::toMsg(q_table); + } + } + + StaticObstacleSpec table_spec; + table_spec.name = "table_proxy"; + table_spec.pose = applyEnvOffset(env_pose, + static_env_offset_x, + static_env_offset_y, + static_env_offset_z, + static_env_offset_roll, + static_env_offset_pitch, + static_env_offset_yaw); + table_spec.size.x = table_proxy_size_x; + table_spec.size.y = table_proxy_size_y; + table_spec.size.z = table_proxy_size_z; + specs.push_back(table_spec); + break; + } + } + + return specs; + } + + } // namespace + + PickPlaceMultiExecutor::PickPlaceMultiExecutor(rclcpp::NodeOptions const& options) + : Node("pick_place_multi_executor", options) { + RCLCPP_INFO(this->get_logger(), "PickPlaceMultiExecutor node initialized"); + + this->declare_parameter("pick_place_multi_executor.object_pose_topic", "/segmentation/object_poses"); + this->declare_parameter("pick_place_multi_executor.base_frame", "panda_link0"); + this->declare_parameter("pick_place_multi_executor.arm_group_name", "panda_arm"); + this->declare_parameter("pick_place_multi_executor.end_effector_link", "panda_hand_tcp"); + this->declare_parameter("pick_place_multi_executor.hand_group_name", "hand"); + this->declare_parameter("pick_place_multi_executor.home_named_target", "ready"); + this->declare_parameter("pick_place_multi_executor.max_objects", 3); + this->declare_parameter("pick_place_multi_executor.approach_offset_z", 0.1); + this->declare_parameter("pick_place_multi_executor.post_grasp_lift_z", 0.1); + this->declare_parameter("pick_place_multi_executor.cartesian_step", 0.01); + this->declare_parameter("pick_place_multi_executor.min_cartesian_fraction", 0.9); + this->declare_parameter("pick_place_multi_executor.enforce_min_z", true); + this->declare_parameter("pick_place_multi_executor.min_allowed_z", 0.0); + this->declare_parameter("pick_place_multi_executor.use_current_ee_orientation_for_pick", false); + this->declare_parameter("pick_place_multi_executor.use_fixed_pick_orientation", true); + this->declare_parameter("pick_place_multi_executor.pick_pose.qx", 1.0); + this->declare_parameter("pick_place_multi_executor.pick_pose.qy", 0.0); + this->declare_parameter("pick_place_multi_executor.pick_pose.qz", 0.0); + this->declare_parameter("pick_place_multi_executor.pick_pose.qw", 0.0); + this->declare_parameter("pick_place_multi_executor.execute_enabled", false); + this->declare_parameter("pick_place_multi_executor.execute_pick_only", false); + this->declare_parameter("pick_place_multi_executor.continue_on_failure", true); + + this->declare_parameter("pick_place_multi_executor.place_poses_x", std::vector{0.35, 0.42, 0.49}); + this->declare_parameter("pick_place_multi_executor.place_poses_y", std::vector{-0.30, -0.30, -0.30}); + this->declare_parameter("pick_place_multi_executor.place_poses_z", std::vector{0.20, 0.20, 0.20}); + this->declare_parameter("pick_place_multi_executor.place_poses_qx", std::vector{1.0, 1.0, 1.0}); + this->declare_parameter("pick_place_multi_executor.place_poses_qy", std::vector{0.0, 0.0, 0.0}); + this->declare_parameter("pick_place_multi_executor.place_poses_qz", std::vector{0.0, 0.0, 0.0}); + this->declare_parameter("pick_place_multi_executor.place_poses_qw", std::vector{0.0, 0.0, 0.0}); + + this->declare_parameter("pick_place_multi_executor.publish_static_env_obstacles", true); + this->declare_parameter("pick_place_multi_executor.static_env_sdf_path", + "src/dev_robotic_arm_engineering/src/panda/worlds/multi_object_sequential_env.sdf"); + this->declare_parameter("pick_place_multi_executor.static_env_offset_x", 0.0); + this->declare_parameter("pick_place_multi_executor.static_env_offset_y", -1.70); + this->declare_parameter("pick_place_multi_executor.static_env_offset_z", -1.0); + this->declare_parameter("pick_place_multi_executor.static_env_offset_roll", 0.0); + this->declare_parameter("pick_place_multi_executor.static_env_offset_pitch", 0.0); + this->declare_parameter("pick_place_multi_executor.static_env_offset_yaw", 0.0); + this->declare_parameter("pick_place_multi_executor.table_proxy_enabled", true); + this->declare_parameter("pick_place_multi_executor.table_proxy_size_x", 1.50); + this->declare_parameter("pick_place_multi_executor.table_proxy_size_y", 0.80); + this->declare_parameter("pick_place_multi_executor.table_proxy_size_z", 0.08); + this->declare_parameter("pick_place_multi_executor.table_proxy_center_z_offset", 1.0); + + object_pose_topic_ = this->get_parameter("pick_place_multi_executor.object_pose_topic").as_string(); + base_frame_ = this->get_parameter("pick_place_multi_executor.base_frame").as_string(); + arm_group_name_ = this->get_parameter("pick_place_multi_executor.arm_group_name").as_string(); + end_effector_link_ = this->get_parameter("pick_place_multi_executor.end_effector_link").as_string(); + hand_group_name_ = this->get_parameter("pick_place_multi_executor.hand_group_name").as_string(); + home_named_target_ = this->get_parameter("pick_place_multi_executor.home_named_target").as_string(); + max_objects_ = this->get_parameter("pick_place_multi_executor.max_objects").as_int(); + approach_offset_z_ = this->get_parameter("pick_place_multi_executor.approach_offset_z").as_double(); + post_grasp_lift_z_ = this->get_parameter("pick_place_multi_executor.post_grasp_lift_z").as_double(); + cartesian_step_ = this->get_parameter("pick_place_multi_executor.cartesian_step").as_double(); + min_cartesian_fraction_ = this->get_parameter("pick_place_multi_executor.min_cartesian_fraction").as_double(); + enforce_min_z_ = this->get_parameter("pick_place_multi_executor.enforce_min_z").as_bool(); + min_allowed_z_ = this->get_parameter("pick_place_multi_executor.min_allowed_z").as_double(); + use_current_ee_orientation_for_pick_ = + this->get_parameter("pick_place_multi_executor.use_current_ee_orientation_for_pick").as_bool(); + use_fixed_pick_orientation_ = this->get_parameter("pick_place_multi_executor.use_fixed_pick_orientation").as_bool(); + execute_enabled_ = this->get_parameter("pick_place_multi_executor.execute_enabled").as_bool(); + execute_pick_only_ = this->get_parameter("pick_place_multi_executor.execute_pick_only").as_bool(); + continue_on_failure_ = this->get_parameter("pick_place_multi_executor.continue_on_failure").as_bool(); + + fixed_pick_orientation_.x = this->get_parameter("pick_place_multi_executor.pick_pose.qx").as_double(); + fixed_pick_orientation_.y = this->get_parameter("pick_place_multi_executor.pick_pose.qy").as_double(); + fixed_pick_orientation_.z = this->get_parameter("pick_place_multi_executor.pick_pose.qz").as_double(); + fixed_pick_orientation_.w = this->get_parameter("pick_place_multi_executor.pick_pose.qw").as_double(); + + publish_static_env_obstacles_ = + this->get_parameter("pick_place_multi_executor.publish_static_env_obstacles").as_bool(); + static_env_sdf_path_ = this->get_parameter("pick_place_multi_executor.static_env_sdf_path").as_string(); + static_env_offset_x_ = this->get_parameter("pick_place_multi_executor.static_env_offset_x").as_double(); + static_env_offset_y_ = this->get_parameter("pick_place_multi_executor.static_env_offset_y").as_double(); + static_env_offset_z_ = this->get_parameter("pick_place_multi_executor.static_env_offset_z").as_double(); + static_env_offset_roll_ = this->get_parameter("pick_place_multi_executor.static_env_offset_roll").as_double(); + static_env_offset_pitch_ = this->get_parameter("pick_place_multi_executor.static_env_offset_pitch").as_double(); + static_env_offset_yaw_ = this->get_parameter("pick_place_multi_executor.static_env_offset_yaw").as_double(); + table_proxy_enabled_ = this->get_parameter("pick_place_multi_executor.table_proxy_enabled").as_bool(); + table_proxy_size_x_ = this->get_parameter("pick_place_multi_executor.table_proxy_size_x").as_double(); + table_proxy_size_y_ = this->get_parameter("pick_place_multi_executor.table_proxy_size_y").as_double(); + table_proxy_size_z_ = this->get_parameter("pick_place_multi_executor.table_proxy_size_z").as_double(); + table_proxy_center_z_offset_ = + this->get_parameter("pick_place_multi_executor.table_proxy_center_z_offset").as_double(); + + auto place_x = this->get_parameter("pick_place_multi_executor.place_poses_x").as_double_array(); + auto place_y = this->get_parameter("pick_place_multi_executor.place_poses_y").as_double_array(); + auto place_z = this->get_parameter("pick_place_multi_executor.place_poses_z").as_double_array(); + auto place_qx = this->get_parameter("pick_place_multi_executor.place_poses_qx").as_double_array(); + auto place_qy = this->get_parameter("pick_place_multi_executor.place_poses_qy").as_double_array(); + auto place_qz = this->get_parameter("pick_place_multi_executor.place_poses_qz").as_double_array(); + auto place_qw = this->get_parameter("pick_place_multi_executor.place_poses_qw").as_double_array(); + + size_t place_count = std::min({place_x.size(), + place_y.size(), + place_z.size(), + place_qx.size(), + place_qy.size(), + place_qz.size(), + place_qw.size()}); + for (size_t i = 0; i < place_count; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = base_frame_; + pose.pose.position.x = place_x[i]; + pose.pose.position.y = place_y[i]; + pose.pose.position.z = place_z[i]; + pose.pose.orientation.x = place_qx[i]; + pose.pose.orientation.y = place_qy[i]; + pose.pose.orientation.z = place_qz[i]; + pose.pose.orientation.w = place_qw[i]; + place_poses_.push_back(pose); + } + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_unique(*tf_buffer_); + + std::vector available_groups; + std::string resolved_arm_group_name; + try { + robot_model_loader::RobotModelLoader model_loader(std::shared_ptr(this, [](rclcpp::Node*) {}), + "robot_description"); + auto model = model_loader.getModel(); + if (model) { + available_groups = model->getJointModelGroupNames(); + resolved_arm_group_name = chooseArmGroupName(available_groups, arm_group_name_); + } + } catch (std::exception const& ex) { + RCLCPP_WARN(this->get_logger(), "Robot model load for group detection failed: %s", ex.what()); + } + + if (!resolved_arm_group_name.empty()) { + if (resolved_arm_group_name != arm_group_name_) { + RCLCPP_WARN(this->get_logger(), + "Requested arm group '%s' not available; using '%s'", + arm_group_name_.c_str(), + resolved_arm_group_name.c_str()); + } + arm_group_name_ = resolved_arm_group_name; + } + + try { + auto node_handle = std::shared_ptr(this, [](rclcpp::Node*) {}); + arm_move_group_ = std::make_shared(node_handle, arm_group_name_); + arm_move_group_->setPoseReferenceFrame(base_frame_); + arm_move_group_->setEndEffectorLink(end_effector_link_); + arm_move_group_->setPlanningTime(5.0); + hand_move_group_ = + std::make_shared(node_handle, hand_group_name_); + hand_move_group_->setPlanningTime(3.0); + } catch (std::exception const& ex) { + arm_move_group_.reset(); + hand_move_group_.reset(); + RCLCPP_ERROR( + this->get_logger(), "MoveGroup initialization failed for group '%s': %s", arm_group_name_.c_str(), ex.what()); + RCLCPP_ERROR(this->get_logger(), "Planning is disabled until arm group configuration is corrected."); + } + + if (!place_poses_.empty()) { + RCLCPP_INFO(this->get_logger(), "Configured %zu place targets", place_poses_.size()); + } else { + RCLCPP_ERROR(this->get_logger(), "No place targets configured. Sequence execution will fail."); + } + + if (!loadStaticObstaclesFromSdf()) { + RCLCPP_WARN(this->get_logger(), "Static obstacle load failed. Planning will continue without those collisions."); + } + + object_poses_sub_ = this->create_subscription( + object_pose_topic_, 10, std::bind(&PickPlaceMultiExecutor::objectPosesCallback, this, std::placeholders::_1)); + + rerun_service_ = this->create_service( + "/pick_place/rerun_multi", + std::bind(&PickPlaceMultiExecutor::rerunServiceCallback, this, std::placeholders::_1, std::placeholders::_2)); + + RCLCPP_INFO(this->get_logger(), "Subscribed to %s", object_pose_topic_.c_str()); + RCLCPP_INFO(this->get_logger(), "Service ready: /pick_place/rerun_multi"); + RCLCPP_INFO(this->get_logger(), "Object order strategy: PoseArray order as published"); + RCLCPP_INFO( + this->get_logger(), "Failure strategy: continue_on_failure=%s", continue_on_failure_ ? "true" : "false"); + + setState(State::WAITING_FOR_SNAPSHOT); + } + + bool PickPlaceMultiExecutor::loadStaticObstaclesFromSdf() { + if (!publish_static_env_obstacles_) { + RCLCPP_INFO(this->get_logger(), "Static environment obstacles disabled by parameter"); + return true; + } + + std::ifstream input(static_env_sdf_path_); + if (!input.is_open()) { + RCLCPP_ERROR(this->get_logger(), "Cannot open SDF file: %s", static_env_sdf_path_.c_str()); + return false; + } + + std::string sdf_text((std::istreambuf_iterator(input)), std::istreambuf_iterator()); + if (sdf_text.empty()) { + RCLCPP_ERROR(this->get_logger(), "SDF file is empty: %s", static_env_sdf_path_.c_str()); + return false; + } + + auto specs = parseStaticObstaclesFromSdf(sdf_text, + table_proxy_enabled_, + table_proxy_size_x_, + table_proxy_size_y_, + table_proxy_size_z_, + table_proxy_center_z_offset_, + static_env_offset_x_, + static_env_offset_y_, + static_env_offset_z_, + static_env_offset_roll_, + static_env_offset_pitch_, + static_env_offset_yaw_); + if (specs.empty()) { + RCLCPP_WARN(this->get_logger(), "No static obstacles parsed from SDF: %s", static_env_sdf_path_.c_str()); + return false; + } + + std::vector ids; + std::vector collision_objects; + collision_objects.reserve(specs.size()); + + for (auto const& spec : specs) { + moveit_msgs::msg::CollisionObject object; + object.id = "env_" + spec.name; + object.header.frame_id = base_frame_; + + shape_msgs::msg::SolidPrimitive primitive; + primitive.type = shape_msgs::msg::SolidPrimitive::BOX; + primitive.dimensions = {spec.size.x, spec.size.y, spec.size.z}; + + object.primitives.push_back(primitive); + object.primitive_poses.push_back(spec.pose); + object.operation = moveit_msgs::msg::CollisionObject::ADD; + + ids.push_back(object.id); + collision_objects.push_back(object); + } + + planning_scene_interface_.removeCollisionObjects(ids); + planning_scene_interface_.applyCollisionObjects(collision_objects); + + RCLCPP_INFO(this->get_logger(), + "Loaded %zu static obstacles from %s (pick targets excluded)", + collision_objects.size(), + static_env_sdf_path_.c_str()); + return true; + } + + bool PickPlaceMultiExecutor::transformPoseToBase(geometry_msgs::msg::PoseStamped const& input, + geometry_msgs::msg::PoseStamped& output) const { + if (input.header.frame_id == base_frame_ || input.header.frame_id.empty()) { + output = input; + output.header.frame_id = base_frame_; + return true; + } + + try { + output = tf_buffer_->transform(input, base_frame_, tf2::durationFromSec(0.2)); + return true; + } catch (tf2::TransformException const& ex) { + RCLCPP_WARN(this->get_logger(), + "Pose transform failed (%s -> %s): %s", + input.header.frame_id.c_str(), + base_frame_.c_str(), + ex.what()); + return false; + } + } + + void PickPlaceMultiExecutor::objectPosesCallback(const geometry_msgs::msg::PoseArray::SharedPtr msg) { + if (snapshot_locked_ || planning_busy_) { + return; + } + if (msg->poses.empty()) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 2000, "PoseArray is empty; waiting for object poses"); + return; + } + if (place_poses_.empty()) { + RCLCPP_ERROR(this->get_logger(), "Cannot capture snapshot because no place targets are configured"); + return; + } + + size_t capture_limit = static_cast(std::max(1, max_objects_)); + capture_limit = std::min(capture_limit, msg->poses.size()); + object_poses_snapshot_.clear(); + object_poses_snapshot_.reserve(capture_limit); + + for (size_t i = 0; i < capture_limit; ++i) { + geometry_msgs::msg::PoseStamped pose; + pose.header = msg->header; + pose.pose = msg->poses[i]; + + geometry_msgs::msg::PoseStamped pose_base; + if (!transformPoseToBase(pose, pose_base)) { + continue; + } + object_poses_snapshot_.push_back(pose_base); + } + + if (object_poses_snapshot_.empty()) { + RCLCPP_WARN(this->get_logger(), "No valid transformed poses captured from PoseArray"); + return; + } + + if (place_poses_.size() < object_poses_snapshot_.size()) { + RCLCPP_ERROR(this->get_logger(), + "Insufficient place targets (%zu) for captured objects (%zu)", + place_poses_.size(), + object_poses_snapshot_.size()); + return; + } + + snapshot_locked_ = true; + setState(State::RUNNING); + RCLCPP_INFO(this->get_logger(), + "Captured one-time snapshot: %zu objects in %s. Starting sequence.", + object_poses_snapshot_.size(), + base_frame_.c_str()); + + planning_busy_ = true; + (void)executeSequence(); + planning_busy_ = false; + } + + bool PickPlaceMultiExecutor::executeSequence() { + if (!arm_move_group_) { + RCLCPP_ERROR(this->get_logger(), "Cannot execute sequence: arm MoveGroup is not initialized"); + setState(State::IDLE); + return false; + } + if (object_poses_snapshot_.empty()) { + RCLCPP_WARN(this->get_logger(), "No snapshot available for execution"); + setState(State::WAITING_FOR_SNAPSHOT); + return false; + } + + size_t success_count = 0; + for (size_t i = 0; i < object_poses_snapshot_.size(); ++i) { + auto const& object_pose = object_poses_snapshot_[i]; + auto const& place_pose = place_poses_[i]; + RCLCPP_INFO(this->get_logger(), "Executing object %zu/%zu", i + 1, object_poses_snapshot_.size()); + + bool ok = runPlanningSequenceForObject(object_pose, place_pose, i); + if (ok) { + success_count++; + continue; + } + + RCLCPP_ERROR(this->get_logger(), "Object %zu failed", i + 1); + (void)planGripperAction("open", execute_enabled_); + (void)planToNamedTarget(home_named_target_, "recover_home", execute_enabled_); + if (!continue_on_failure_) { + break; + } + } + + sequence_completed_once_ = true; + setState(State::COMPLETED); + RCLCPP_INFO(this->get_logger(), + "Sequence finished: %zu/%zu objects succeeded", + success_count, + object_poses_snapshot_.size()); + setState(State::IDLE); + return success_count > 0; + } + + bool PickPlaceMultiExecutor::runPlanningSequenceForObject(geometry_msgs::msg::PoseStamped const& object_pose_base, + geometry_msgs::msg::PoseStamped const& place_pose_base, + size_t object_index) { + bool execute_pick_stages = execute_enabled_; + bool execute_place_stages = execute_enabled_ && !execute_pick_only_; + + std::string index_suffix = "obj_" + std::to_string(object_index + 1); + + if (!planGripperAction("open", execute_pick_stages)) { + return false; + } + + geometry_msgs::msg::PoseStamped pick_pose = object_pose_base; + pick_pose.header.frame_id = base_frame_; + if (use_fixed_pick_orientation_) { + pick_pose.pose.orientation = fixed_pick_orientation_; + } else if (use_current_ee_orientation_for_pick_) { + auto current_pose = arm_move_group_->getCurrentPose(end_effector_link_); + if (!current_pose.header.frame_id.empty()) { + pick_pose.pose.orientation = current_pose.pose.orientation; + } + } + + // Keep tool Z-axis pointing downward in base frame. + { + tf2::Quaternion q_target(pick_pose.pose.orientation.x, + pick_pose.pose.orientation.y, + pick_pose.pose.orientation.z, + pick_pose.pose.orientation.w); + q_target.normalize(); + tf2::Matrix3x3 m_target(q_target); + tf2::Vector3 z_axis_target = m_target.getColumn(2); + if (z_axis_target.z() > 0.0) { + tf2::Quaternion flip_x(1.0, 0.0, 0.0, 0.0); + tf2::Quaternion q_flipped = q_target * flip_x; + q_flipped.normalize(); + pick_pose.pose.orientation = tf2::toMsg(q_flipped); + } + } + + if (enforce_min_z_ && pick_pose.pose.position.z < min_allowed_z_) { + RCLCPP_ERROR(this->get_logger(), + "[%s] Pick pose rejected: z=%.3f below min_allowed_z=%.3f", + index_suffix.c_str(), + pick_pose.pose.position.z, + min_allowed_z_); + return false; + } + + auto pre_grasp_pose = computePreGraspPose(pick_pose); + if (enforce_min_z_ && pre_grasp_pose.pose.position.z < min_allowed_z_) { + RCLCPP_ERROR(this->get_logger(), + "[%s] Pre-grasp pose rejected: z=%.3f below min_allowed_z=%.3f", + index_suffix.c_str(), + pre_grasp_pose.pose.position.z, + min_allowed_z_); + return false; + } + + if (!planToPoseTarget(pre_grasp_pose, "pre_grasp_" + index_suffix, execute_pick_stages)) { + return false; + } + if (!planCartesianMove(pick_pose.pose, "descend_" + index_suffix, execute_pick_stages)) { + return false; + } + if (!planGripperAction("close", execute_pick_stages)) { + return false; + } + geometry_msgs::msg::Pose post_grasp_pose = pick_pose.pose; + post_grasp_pose.position.z += post_grasp_lift_z_; + if (!planCartesianMove(post_grasp_pose, "retreat_" + index_suffix, execute_place_stages)) { + return false; + } + if (!planToPoseTarget(place_pose_base, "place_pose_" + index_suffix, execute_place_stages)) { + return false; + } + if (!planGripperAction("open", execute_place_stages)) { + return false; + } + if (!planToNamedTarget(home_named_target_, "home_" + index_suffix, execute_place_stages)) { + return false; + } + + return true; + } + + geometry_msgs::msg::PoseStamped + PickPlaceMultiExecutor::computePreGraspPose(geometry_msgs::msg::PoseStamped const& object_pose_base) const { + geometry_msgs::msg::PoseStamped pre_grasp_pose = object_pose_base; + pre_grasp_pose.header.frame_id = base_frame_; + pre_grasp_pose.pose.position.z += approach_offset_z_; + return pre_grasp_pose; + } + + bool PickPlaceMultiExecutor::planToPoseTarget(geometry_msgs::msg::PoseStamped const& target_pose, + std::string const& stage_name, + bool execute_after_plan) { + if (enforce_min_z_ && target_pose.pose.position.z < min_allowed_z_) { + RCLCPP_ERROR(this->get_logger(), + "[%s] Target pose rejected: z=%.3f below min_allowed_z=%.3f", + stage_name.c_str(), + target_pose.pose.position.z, + min_allowed_z_); + return false; + } + + arm_move_group_->setStartStateToCurrentState(); + arm_move_group_->setPoseTarget(target_pose, end_effector_link_); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = static_cast(arm_move_group_->plan(plan)); + arm_move_group_->clearPoseTargets(); + + if (!success) { + RCLCPP_ERROR(this->get_logger(), "Planning failed: %s", stage_name.c_str()); + return false; + } + + if (execute_after_plan) { + return executeArmPlan(plan, stage_name); + } + return true; + } + + bool PickPlaceMultiExecutor::planCartesianMove(geometry_msgs::msg::Pose const& target_pose, + std::string const& stage_name, + bool execute_after_plan) { + if (enforce_min_z_ && target_pose.position.z < min_allowed_z_) { + RCLCPP_ERROR(this->get_logger(), + "[%s] Cartesian target rejected: z=%.3f below min_allowed_z=%.3f", + stage_name.c_str(), + target_pose.position.z, + min_allowed_z_); + return false; + } + + arm_move_group_->setStartStateToCurrentState(); + std::vector waypoints{target_pose}; + moveit_msgs::msg::RobotTrajectory trajectory; + double fraction = arm_move_group_->computeCartesianPath(waypoints, cartesian_step_, 0.0, trajectory); + if (fraction < min_cartesian_fraction_) { + RCLCPP_ERROR(this->get_logger(), + "Cartesian planning failed: %s | fraction=%.2f < required %.2f", + stage_name.c_str(), + fraction, + min_cartesian_fraction_); + return false; + } + + if (!execute_after_plan) { + return true; + } + + moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan; + cartesian_plan.trajectory = trajectory; + return executeArmPlan(cartesian_plan, stage_name); + } + + bool PickPlaceMultiExecutor::planToNamedTarget(std::string const& target_name, + std::string const& stage_name, + bool execute_after_plan) { + arm_move_group_->setStartStateToCurrentState(); + arm_move_group_->setNamedTarget(target_name); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + bool success = static_cast(arm_move_group_->plan(plan)); + if (!success) { + RCLCPP_ERROR(this->get_logger(), "Planning failed: %s (%s)", stage_name.c_str(), target_name.c_str()); + return false; + } + + if (execute_after_plan) { + return executeArmPlan(plan, stage_name); + } + return true; + } + + bool PickPlaceMultiExecutor::planGripperAction(std::string const& action_name, bool execute_action) { + if (!execute_action) { + RCLCPP_INFO(this->get_logger(), "Gripper action (plan-only): %s", action_name.c_str()); + return true; + } + return executeHandNamedTarget(action_name, "gripper_" + action_name); + } + + bool PickPlaceMultiExecutor::executeArmPlan(moveit::planning_interface::MoveGroupInterface::Plan const& plan, + std::string const& stage_name) { + auto result = arm_move_group_->execute(plan); + if (result == moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_INFO(this->get_logger(), "Execution success: %s", stage_name.c_str()); + return true; + } + RCLCPP_ERROR(this->get_logger(), "Execution failed: %s (error_code=%d)", stage_name.c_str(), result.val); + return false; + } + + bool PickPlaceMultiExecutor::executeHandNamedTarget(std::string const& target_name, std::string const& stage_name) { + if (!hand_move_group_) { + RCLCPP_ERROR(this->get_logger(), "Gripper execution failed: hand MoveGroup is not initialized"); + return false; + } + + hand_move_group_->setStartStateToCurrentState(); + hand_move_group_->setNamedTarget(target_name); + moveit::planning_interface::MoveGroupInterface::Plan hand_plan; + bool planned = static_cast(hand_move_group_->plan(hand_plan)); + if (!planned) { + RCLCPP_WARN(this->get_logger(), + "Gripper named-target planning failed: %s (%s). Trying direct joint fallback.", + stage_name.c_str(), + target_name.c_str()); + + auto const active_joints = hand_move_group_->getActiveJoints(); + if (active_joints.empty()) { + RCLCPP_ERROR(this->get_logger(), "Gripper fallback failed: no active joints in hand group"); + return false; + } + + double finger_target = 0.04; + if (target_name == "close") { + finger_target = 0.0; + } else if (target_name == "open") { + finger_target = 0.04; + } + + std::map joint_targets; + for (auto const& joint_name : active_joints) { + joint_targets[joint_name] = finger_target; + } + + bool set_target_ok = hand_move_group_->setJointValueTarget(joint_targets); + if (!set_target_ok) { + RCLCPP_ERROR(this->get_logger(), "Gripper fallback failed: setJointValueTarget returned false"); + return false; + } + + planned = static_cast(hand_move_group_->plan(hand_plan)); + if (!planned) { + RCLCPP_ERROR(this->get_logger(), + "Gripper planning failed after fallback: %s (%s)", + stage_name.c_str(), + target_name.c_str()); + return false; + } + } + + auto result = hand_move_group_->execute(hand_plan); + if (result == moveit::core::MoveItErrorCode::SUCCESS) { + RCLCPP_INFO(this->get_logger(), "Gripper execution success: %s (%s)", stage_name.c_str(), target_name.c_str()); + return true; + } + + RCLCPP_ERROR(this->get_logger(), + "Gripper execution failed: %s (%s), error_code=%d", + stage_name.c_str(), + target_name.c_str(), + result.val); + return false; + } + + void PickPlaceMultiExecutor::rerunServiceCallback(const std::shared_ptr request, + std::shared_ptr response) { + (void)request; + if (planning_busy_) { + response->success = false; + response->message = "Planning already in progress"; + return; + } + if (object_poses_snapshot_.empty()) { + response->success = false; + response->message = "No captured snapshot available yet"; + return; + } + + setState(State::RUNNING); + planning_busy_ = true; + bool ok = executeSequence(); + planning_busy_ = false; + + response->success = ok; + response->message = ok ? "Rerun accepted and sequence executed" : "Rerun executed with no successful objects"; + } + + void PickPlaceMultiExecutor::setState(State new_state) { + if (state_ == new_state) { + return; + } + RCLCPP_INFO(this->get_logger(), "State transition: %s -> %s", stateToString(state_), stateToString(new_state)); + state_ = new_state; + } + + char const* PickPlaceMultiExecutor::stateToString(State state) const { + switch (state) { + case State::IDLE: + return "IDLE"; + case State::WAITING_FOR_SNAPSHOT: + return "WAITING_FOR_SNAPSHOT"; + case State::RUNNING: + return "RUNNING"; + case State::COMPLETED: + return "COMPLETED"; + default: + return "UNKNOWN"; + } + } + +} // namespace pick_place diff --git a/src/pick_place/src/pick_place_multi_executor_main.cpp b/src/pick_place/src/pick_place_multi_executor_main.cpp new file mode 100644 index 0000000..6743dd0 --- /dev/null +++ b/src/pick_place/src/pick_place_multi_executor_main.cpp @@ -0,0 +1,12 @@ +#include "pick_place/pick_place_multi_executor.hpp" + +#include + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + RCLCPP_INFO(node->get_logger(), "Starting pick_place_multi_executor_node..."); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/segmentation/config/segmentation_params.yaml b/src/segmentation/config/segmentation_params.yaml index e49f22a..8a3eba1 100644 --- a/src/segmentation/config/segmentation_params.yaml +++ b/src/segmentation/config/segmentation_params.yaml @@ -17,6 +17,11 @@ # Show debug windows with masks (set to false in production) debug_images: false + # Camera topics (configurable per setup) + rgb_topic: "/wrist_eye/image_raw" + depth_topic: "/wrist_eye/depth/image_raw" + camera_info_topic: "/wrist_eye/depth/camera_info" + # Phase 2: Depth processing parameters # Enable/disable 3D localization using depth camera depth_processing_enabled: true @@ -37,15 +42,13 @@ # Options: "world", "panda_link0" (both are equivalent for this robot) target_frame: "panda_link0" - # Source frame (camera optical frame) - # The camera publishes images with frame_id in the header - # Must match the actual frame name in TF tree - # Use optical frame so depth Z-axis points in correct direction - source_frame: "panda_wrist_eye_optical_frame" + # Source frame used for deprojection and marker/point cloud frame IDs. + # This should be an optical frame. If your camera only has a non-optical + # frame, use launch to publish a static transform to this optical frame. + source_frame: "camera_optical_frame" - # Use camera frame from image header instead of source_frame parameter - # If true: uses header.frame_id from camera images - # If false: always uses source_frame parameter above (RECOMMENDED) + # Keep false when image headers use non-optical camera frames. + # Set true only if header.frame_id is already an optical frame. use_image_frame: false # TF lookup timeout in seconds diff --git a/src/segmentation/launch/segmentation.launch.py b/src/segmentation/launch/segmentation.launch.py index e6fe2e6..a0007c7 100644 --- a/src/segmentation/launch/segmentation.launch.py +++ b/src/segmentation/launch/segmentation.launch.py @@ -1,13 +1,11 @@ #!/usr/bin/env python3 from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, LogInfo +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_share_directory -import os -import math def generate_launch_description(): @@ -27,20 +25,56 @@ def generate_launch_description(): "use_sim_time", default_value="true", description="Use simulation time" ) - # Static transform: Create optical frame from camera link - # The camera link has: X=down, Y=left, Z=forward (gripper perspective) - # Optical frame needs: X=right, Y=down, Z=forward (depth forward = down to table) - # - # Transform chain: - # 1. Rotate 90° around Z to swap X and Y: X=left, Y=down, Z=forward - # 2. Rotate 180° around Y to flip X: X=right, Y=down, Z=forward - # Combined: quat from (0, 90, 180) = (-0.5, 0.5, 0.5, 0.5) - # - # Alternative interpretation (if above doesn't work): - # Camera X=down → Optical Z - # Camera Y=left → Optical Y - # Camera Z=forward → Optical X - # This is: 90° around Y = (0, 0.7071068, 0, 0.7071068) + camera_frame_arg = DeclareLaunchArgument( + "camera_frame", + default_value="panda_wrist_eye_sensor", + description="Non-optical camera frame that depth images originate from", + ) + + camera_header_frame_arg = DeclareLaunchArgument( + "camera_header_frame", + default_value="Panda/panda_link7/panda_wrist_eye_sensor", + description="Frame ID found in incoming image headers", + ) + + publish_header_alias_tf_arg = DeclareLaunchArgument( + "publish_header_alias_tf", + default_value="true", + description="Publish identity TF from camera_header_frame to camera_frame", + ) + + camera_optical_frame_arg = DeclareLaunchArgument( + "camera_optical_frame", + default_value="camera_optical_frame", + description="Optical frame used by segmentation for depth deprojection", + ) + + publish_optical_tf_arg = DeclareLaunchArgument( + "publish_optical_tf", + default_value="true", + description="Publish static TF from non-optical camera frame to optical frame", + ) + + camera_header_alias_publisher = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="camera_header_alias_publisher", + arguments=[ + "0", + "0", + "0", + "0", + "0", + "0", + "1", + LaunchConfiguration("camera_header_frame"), + LaunchConfiguration("camera_frame"), + ], + parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}], + condition=IfCondition(LaunchConfiguration("publish_header_alias_tf")), + ) + + # REP-103 optical frame convention: x-right, y-down, z-forward. camera_optical_frame_publisher = Node( package="tf2_ros", executable="static_transform_publisher", @@ -48,17 +82,16 @@ def generate_launch_description(): arguments=[ "0", "0", - "0", # translation (x, y, z) - # Option 1: 90° around Y (current) - "0", - "0.7071068", "0", - "0.7071068", - # Option 2: Try (-0.5, 0.5, 0.5, 0.5) if positions still wrong - "panda_wrist_eye_link", - "panda_wrist_eye_optical_frame", + "-0.5", + "0.5", + "-0.5", + "0.5", + LaunchConfiguration("camera_frame"), + LaunchConfiguration("camera_optical_frame"), ], parameters=[{"use_sim_time": LaunchConfiguration("use_sim_time")}], + condition=IfCondition(LaunchConfiguration("publish_optical_tf")), ) # Color segmentation node @@ -70,13 +103,15 @@ def generate_launch_description(): parameters=[ LaunchConfiguration("config_file"), {"use_sim_time": LaunchConfiguration("use_sim_time")}, + {"source_frame": LaunchConfiguration("camera_optical_frame")}, ], remappings=[ - # Input topics - ("/wrist_eye/image_raw", "/wrist_eye/image_raw"), # Output topics ("~/annotated_image", "/segmentation/annotated_image"), ("~/detection_markers", "/segmentation/detection_markers"), + ("~/object_poses", "/segmentation/object_poses"), + ("~/detections", "/segmentation/detections"), + ("~/detected_objects_cloud", "/segmentation/detected_objects_cloud"), ], arguments=["--ros-args", "--log-level", "info"], ) @@ -85,7 +120,12 @@ def generate_launch_description(): [ config_file_arg, use_sim_time_arg, - LogInfo(msg="Starting Color Segmentation Node with Optical Frame"), + camera_frame_arg, + camera_header_frame_arg, + publish_header_alias_tf_arg, + camera_optical_frame_arg, + publish_optical_tf_arg, + camera_header_alias_publisher, camera_optical_frame_publisher, color_segmentation_node, ] diff --git a/src/segmentation/src/color_segmentation_node.cpp b/src/segmentation/src/color_segmentation_node.cpp index 09c625d..5c80173 100644 --- a/src/segmentation/src/color_segmentation_node.cpp +++ b/src/segmentation/src/color_segmentation_node.cpp @@ -1,14 +1,15 @@ +#include #include #include #include #include #include #include -#include > #include #include #include #include +#include #include #include #include @@ -53,6 +54,9 @@ class ColorSegmentationNode : public rclcpp::Node { this->declare_parameter("morph_kernel_size", 5); this->declare_parameter("visualization_enabled", true); this->declare_parameter("debug_images", false); + this->declare_parameter("rgb_topic", "/camera/image_raw"); + this->declare_parameter("depth_topic", "/camera/depth/image_raw"); + this->declare_parameter("camera_info_topic", "/camera/depth/camera_info"); // Phase 2 parameters this->declare_parameter("depth_processing_enabled", true); @@ -75,6 +79,9 @@ class ColorSegmentationNode : public rclcpp::Node { morph_kernel_size_ = this->get_parameter("morph_kernel_size").as_int(); visualization_enabled_ = this->get_parameter("visualization_enabled").as_bool(); debug_images_ = this->get_parameter("debug_images").as_bool(); + rgb_topic_ = this->get_parameter("rgb_topic").as_string(); + depth_topic_ = this->get_parameter("depth_topic").as_string(); + camera_info_topic_ = this->get_parameter("camera_info_topic").as_string(); // Phase 2 parameters depth_processing_enabled_ = this->get_parameter("depth_processing_enabled").as_bool(); @@ -94,19 +101,15 @@ class ColorSegmentationNode : public rclcpp::Node { // Create subscribers image_sub_ = this->create_subscription( - "/wrist_eye/image_raw", 10, std::bind(&ColorSegmentationNode::imageCallback, this, std::placeholders::_1)); + rgb_topic_, 10, std::bind(&ColorSegmentationNode::imageCallback, this, std::placeholders::_1)); // Phase 2: Subscribe to depth and camera_info if (depth_processing_enabled_) { depth_sub_ = this->create_subscription( - "/wrist_eye/depth/image_raw", - 10, - std::bind(&ColorSegmentationNode::depthCallback, this, std::placeholders::_1)); + depth_topic_, 10, std::bind(&ColorSegmentationNode::depthCallback, this, std::placeholders::_1)); camera_info_sub_ = this->create_subscription( - "/wrist_eye/depth/camera_info", - 10, - std::bind(&ColorSegmentationNode::cameraInfoCallback, this, std::placeholders::_1)); + camera_info_topic_, 10, std::bind(&ColorSegmentationNode::cameraInfoCallback, this, std::placeholders::_1)); } // Create publishers @@ -121,24 +124,37 @@ class ColorSegmentationNode : public rclcpp::Node { // Phase 3: Publish pose array and initialize TF if (transform_enabled_) { - pose_array_pub_ = this->create_publisher("~/object_poses_base", 10); + pose_array_pub_ = this->create_publisher("~/object_poses", 10); tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); } RCLCPP_INFO(this->get_logger(), "Color Segmentation Node initialized"); - RCLCPP_INFO(this->get_logger(), "Subscribing to: /wrist_eye/image_raw"); + RCLCPP_INFO(this->get_logger(), "Subscribing to RGB topic: %s", rgb_topic_.c_str()); if (depth_processing_enabled_) { RCLCPP_INFO(this->get_logger(), "Depth processing enabled"); - RCLCPP_INFO(this->get_logger(), "Subscribing to: /wrist_eye/depth/image_raw, /wrist_eye/depth/camera_info"); + RCLCPP_INFO( + this->get_logger(), "Subscribing to depth topics: %s, %s", depth_topic_.c_str(), camera_info_topic_.c_str()); } if (transform_enabled_) { - RCLCPP_INFO(this->get_logger(), "Transform enabled: camera frame → %s", target_frame_.c_str()); + RCLCPP_INFO(this->get_logger(), + "Transform enabled: source=%s (use_image_frame=%s) -> target=%s", + source_frame_.c_str(), + use_image_frame_ ? "true" : "false", + target_frame_.c_str()); } - RCLCPP_INFO(this->get_logger(), "Publishing to: ~/annotated_image, ~/detection_markers, ~/detections"); + RCLCPP_INFO(this->get_logger(), + "Publishing to: ~/annotated_image, ~/detection_markers, ~/detections, ~/object_poses"); } private: + std::string resolveSourceFrame(std_msgs::msg::Header const& header) const { + if (use_image_frame_ && !header.frame_id.empty()) { + return header.frame_id; + } + return source_frame_; + } + void initializeColorRanges() { // Red cube - Note: Red wraps around in HSV, so we need two ranges color_ranges_.push_back({ @@ -527,12 +543,13 @@ class ColorSegmentationNode : public rclcpp::Node { void publishMarkers(std::vector const& detections, std_msgs::msg::Header const& header) { visualization_msgs::msg::MarkerArray marker_array; + auto const source_frame = resolveSourceFrame(header); int id = 0; for (auto const& detection : detections) { visualization_msgs::msg::Marker marker; marker.header = header; - marker.header.frame_id = source_frame_; // Use configured camera frame + marker.header.frame_id = source_frame; marker.ns = "detections"; marker.id = id++; marker.type = visualization_msgs::msg::Marker::SPHERE; @@ -665,7 +682,7 @@ class ColorSegmentationNode : public rclcpp::Node { // Create point cloud message sensor_msgs::msg::PointCloud2 cloud_msg; cloud_msg.header = header; - cloud_msg.header.frame_id = source_frame_; // Use configured camera frame + cloud_msg.header.frame_id = resolveSourceFrame(header); // Define fields: x, y, z, rgb cloud_msg.height = 1; @@ -755,12 +772,7 @@ class ColorSegmentationNode : public rclcpp::Node { } // Determine source frame: use image header or configured parameter - std::string source_frame; - if (use_image_frame_ && !header.frame_id.empty()) { - source_frame = header.frame_id; - } else { - source_frame = source_frame_; // Use configured camera frame - } + auto const source_frame = resolveSourceFrame(header); RCLCPP_DEBUG(this->get_logger(), "Transforming %zu detections from %s to %s", @@ -866,6 +878,9 @@ class ColorSegmentationNode : public rclcpp::Node { int morph_kernel_size_; bool visualization_enabled_; bool debug_images_; + std::string rgb_topic_; + std::string depth_topic_; + std::string camera_info_topic_; // Phase 2 parameters bool depth_processing_enabled_;