From 2d6378d43c8d9192381ace046e4190a8f5d54f66 Mon Sep 17 00:00:00 2001 From: motty Date: Mon, 29 Jun 2026 10:06:49 +0900 Subject: [PATCH 1/8] =?UTF-8?q?=E3=83=AA=E3=83=95=E3=82=A1=E3=82=AF?= =?UTF-8?q?=E3=82=BF=E3=83=AA=E3=83=B3=E3=82=B0=E3=81=AE=E5=AE=9F=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples/src/aruco_detection.cpp | 12 +- crane_plus_examples/src/gripper_control.cpp | 68 ++- crane_plus_examples/src/joint_values.cpp | 83 +++- crane_plus_examples/src/pick_and_place.cpp | 235 ++++++----- crane_plus_examples/src/pick_and_place_tf.cpp | 86 ++-- crane_plus_examples/src/pose_groupstate.cpp | 58 ++- .../crane_plus_examples_py/gripper_control.py | 95 ++--- .../crane_plus_examples_py/joint_values.py | 101 ++--- .../crane_plus_examples_py/pick_and_place.py | 399 ++++++------------ .../pick_and_place_tf.py | 158 +++---- .../crane_plus_examples_py/pose_groupstate.py | 69 ++- 11 files changed, 664 insertions(+), 700 deletions(-) diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index 77ca11d3..bcdb626a 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -80,13 +80,12 @@ class ImageSubscriber : public rclcpp::Node // マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 if (n_markers > 0) { - for (int i = 0; i < n_markers; i++) { - // マーカの回転ベクトルと位置ベクトル - std::vector rvecs, tvecs; - // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 - cv::aruco::estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs); + // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + std::vector rvecs, tvecs; + cv::aruco::estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs); + for (int i = 0; i < n_markers; i++) { // tfの配信 geometry_msgs::msg::TransformStamped t; t.header = msg->header; @@ -94,6 +93,7 @@ class ImageSubscriber : public rclcpp::Node t.transform.translation.x = tvecs[i][0]; t.transform.translation.y = tvecs[i][1]; t.transform.translation.z = tvecs[i][2]; + // 回転ベクトルをクォータニオンに変換 tf2::Quaternion q; cv::Mat cv_rotation_matrix; cv::Rodrigues(rvecs[i], cv_rotation_matrix); diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp index 66cea244..d6245228 100644 --- a/crane_plus_examples/src/gripper_control.cpp +++ b/crane_plus_examples/src/gripper_control.cpp @@ -17,15 +17,43 @@ // /run_move_group/src/run_move_group.cpp #include +#include #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control"); +class GripperController : public rclcpp::Node +{ +public: + explicit GripperController(const rclcpp::NodeOptions & node_options) + : Node("gripper_control", node_options) + { + } + + // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す + void initializeMoveGroup() + { + move_group_gripper_ = std::make_shared(shared_from_this(), "gripper"); + move_group_gripper_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + } + + // グリッパの開閉角度を設定して動かす + void setGripperAngle(const double angle) + { + auto joint_values = move_group_gripper_->getCurrentJointValues(); + joint_values[0] = angle; + move_group_gripper_->setJointValueTarget(joint_values); + move_group_gripper_->move(); + } -double to_radians(const double deg_angle) +private: + std::shared_ptr move_group_gripper_; +}; + +double toRadians(const double deg_angle) { return deg_angle * M_PI / 180.0; } @@ -35,30 +63,30 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); - // For current state monitor - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(move_group_gripper_node); - std::thread([&executor]() {executor.spin();}).detach(); - MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 - move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + auto gripper_controller = std::make_shared(node_options); + + // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる + std::thread spin_thread([gripper_controller]() { + rclcpp::spin(gripper_controller); + }); - auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); + gripper_controller->initializeMoveGroup(); - gripper_joint_values[0] = to_radians(30); - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); + // グリッパを閉じる + gripper_controller->setGripperAngle(toRadians(30.0)); - gripper_joint_values[0] = to_radians(-30); - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); + // グリッパを開く + gripper_controller->setGripperAngle(toRadians(-30.0)); - gripper_joint_values[0] = to_radians(0); - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); + // グリッパを0度にする + gripper_controller->setGripperAngle(toRadians(0.0)); + // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + return 0; } diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp index 609bc4b5..eb5609fa 100644 --- a/crane_plus_examples/src/joint_values.cpp +++ b/crane_plus_examples/src/joint_values.cpp @@ -17,15 +17,55 @@ // /run_move_group/src/run_move_group.cpp #include +#include +#include #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("joint_values"); +class JointValuesController : public rclcpp::Node +{ +public: + explicit JointValuesController(const rclcpp::NodeOptions & node_options) + : Node("joint_values", node_options) + { + } + + // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す + void initializeMoveGroup() + { + move_group_arm_ = std::make_shared(shared_from_this(), "arm_tcp"); + move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + } + + // SRDFに定義されている名前付きの姿勢に移動する + void moveArmToNamedPose(const std::string & name) + { + move_group_arm_->setNamedTarget(name); + move_group_arm_->move(); + } + + // 指定した関節角度にアームを動かす + void moveArmToJointValues(const std::vector & joint_values) + { + move_group_arm_->setJointValueTarget(joint_values); + move_group_arm_->move(); + } -double to_radians(const double deg_angle) + // 現在の関節角度を取得する + std::vector getCurrentJointValues() + { + return move_group_arm_->getCurrentJointValues(); + } + +private: + std::shared_ptr move_group_arm_; +}; + +double toRadians(const double deg_angle) { return deg_angle * M_PI / 180.0; } @@ -35,30 +75,35 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - auto move_group_node = rclcpp::Node::make_shared("joint_values", node_options); - // For current state monitor - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(move_group_node); - std::thread([&executor]() {executor.spin();}).detach(); - MoveGroupInterface move_group_arm(move_group_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 - move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + auto arm_controller = std::make_shared(node_options); + + // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる + std::thread spin_thread([arm_controller]() { + rclcpp::spin(arm_controller); + }); - move_group_arm.setNamedTarget("vertical"); - move_group_arm.move(); + arm_controller->initializeMoveGroup(); - auto joint_values = move_group_arm.getCurrentJointValues(); - double target_joint_value = to_radians(45.0); + // verticalの姿勢から開始 + arm_controller->moveArmToNamedPose("vertical"); + + // 各関節を順番に45度に動かす + auto joint_values = arm_controller->getCurrentJointValues(); + const double TARGET_ANGLE = toRadians(45.0); for (size_t i = 0; i < joint_values.size(); i++) { - joint_values[i] = target_joint_value; - move_group_arm.setJointValueTarget(joint_values); - move_group_arm.move(); + joint_values[i] = TARGET_ANGLE; + arm_controller->moveArmToJointValues(joint_values); } - move_group_arm.setNamedTarget("vertical"); - move_group_arm.move(); + // verticalの姿勢に戻る + arm_controller->moveArmToNamedPose("vertical"); + // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + return 0; } diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp index c5d51168..7bf83d2a 100644 --- a/crane_plus_examples/src/pick_and_place.cpp +++ b/crane_plus_examples/src/pick_and_place.cpp @@ -17,6 +17,7 @@ // /run_move_group/src/run_move_group.cpp #include +#include #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -26,114 +27,148 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place"); - -double to_radians(const double deg_angle) +class PickAndPlaceController : public rclcpp::Node { - return deg_angle * M_PI / 180.0; -} +public: + explicit PickAndPlaceController(const rclcpp::NodeOptions & node_options) + : Node("pick_and_place", node_options) + { + // グリッパの開閉角 + GRIPPER_DEFAULT = 0.0; + GRIPPER_OPEN = toRadians(-30.0); + GRIPPER_CLOSE = toRadians(10.0); + + // 物体の頭上の位置 + ABOVE_POSE = makePose(0.0, -0.21, 0.17, 0.0, 90.0, -90.0); + // 物体を掴む直前・直後の位置（持ち上げた高さ） + PRE_AND_POST_GRASP_POSE = makePose(0.0, -0.1, 0.05, 0.0, 180.0, -90.0); + // 物体を掴む位置 + GRASP_POSE = makePose(0.0, -0.1, 0.02, 0.0, 180.0, -90.0); + // 物体を置く位置 + RELEASE_POSE = makePose(0.25, 0.0, 0.06, 0.0, 90.0, 0.0); + } + + // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す + void initializeMoveGroup() + { + move_group_arm_ = std::make_shared(shared_from_this(), "arm_tcp"); + move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + // IKの成功率を向上させるため、目標位置姿勢の許容範囲を小さく設定 + move_group_arm_->setGoalPositionTolerance(1e-5); + move_group_arm_->setGoalOrientationTolerance(1e-4); + + move_group_gripper_ = std::make_shared(shared_from_this(), "gripper"); + move_group_gripper_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + } + + // SRDFに定義されている名前付きの姿勢に移動する + void moveArmToNamedPose(const std::string & name) + { + move_group_arm_->setNamedTarget(name); + move_group_arm_->move(); + } + + // 指定した位置姿勢にアームを動かす + void moveArmToPose(const geometry_msgs::msg::Pose & pose) + { + move_group_arm_->setPoseTarget(pose); + move_group_arm_->move(); + } + + // グリッパの開閉角度を設定して動かす + void setGripperAngle(const double angle) + { + auto joint_values = move_group_gripper_->getCurrentJointValues(); + joint_values[0] = angle; + move_group_gripper_->setJointValueTarget(joint_values); + move_group_gripper_->move(); + } + + // 目標位置姿勢の定数 + geometry_msgs::msg::Pose ABOVE_POSE; + geometry_msgs::msg::Pose PRE_AND_POST_GRASP_POSE; + geometry_msgs::msg::Pose GRASP_POSE; + geometry_msgs::msg::Pose RELEASE_POSE; + + // グリッパ角度の定数 + double GRIPPER_DEFAULT; + double GRIPPER_OPEN; + double GRIPPER_CLOSE; + +private: + // x, y, z[m]とroll, pitch, yaw[deg]からPoseを生成する + static geometry_msgs::msg::Pose makePose( + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw) + { + geometry_msgs::msg::Pose pose; + pose.position.x = x; + pose.position.y = y; + pose.position.z = z; + tf2::Quaternion q; + q.setRPY(toRadians(roll), toRadians(pitch), toRadians(yaw)); + pose.orientation = tf2::toMsg(q); + return pose; + } + + static double toRadians(const double deg_angle) + { + return deg_angle * M_PI / 180.0; + } + + std::shared_ptr move_group_arm_; + std::shared_ptr move_group_gripper_; +}; int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); - auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); - // For current state monitor - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(move_group_arm_node); - executor.add_node(move_group_gripper_node); - std::thread([&executor]() {executor.spin();}).detach(); - - MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 - move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 - - MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 - move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 - auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); - double GRIPPER_DEFAULT = 0.0; - double GRIPPER_OPEN = to_radians(-30); - double GRIPPER_CLOSE = to_radians(10); - - // Set goal tolerances to improve IK success rate - move_group_arm.setGoalPositionTolerance(1e-5); - move_group_arm.setGoalOrientationTolerance(1e-4); - - move_group_arm.setNamedTarget("vertical"); - move_group_arm.move(); - - gripper_joint_values[0] = GRIPPER_DEFAULT; - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); - - // ----- Picking Preparation ----- - move_group_arm.setNamedTarget("home"); - move_group_arm.move(); - - gripper_joint_values[0] = GRIPPER_OPEN; - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); - - geometry_msgs::msg::Pose target_pose; - tf2::Quaternion q; - target_pose.position.x = 0.0; - target_pose.position.y = -0.21; - target_pose.position.z = 0.17; - q.setRPY(to_radians(0), to_radians(90), to_radians(-90)); - target_pose.orientation = tf2::toMsg(q); - move_group_arm.setPoseTarget(target_pose); - move_group_arm.move(); - - target_pose.position.y = -0.1; - target_pose.position.z = 0.05; - q.setRPY(to_radians(0), to_radians(180), to_radians(-90)); - target_pose.orientation = tf2::toMsg(q); - move_group_arm.setPoseTarget(target_pose); - move_group_arm.move(); - - target_pose.position.z = 0.02; - move_group_arm.setPoseTarget(target_pose); - move_group_arm.move(); - - // Grasp - gripper_joint_values[0] = GRIPPER_CLOSE; - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); - - target_pose.position.z = 0.05; - move_group_arm.setPoseTarget(target_pose); - move_group_arm.move(); - - // ----- Placing Preparation ----- - move_group_arm.setNamedTarget("home"); - move_group_arm.move(); - - target_pose.position.x = 0.25; - target_pose.position.y = 0.0; - target_pose.position.z = 0.06; - q.setRPY(to_radians(0), to_radians(90), to_radians(0)); - target_pose.orientation = tf2::toMsg(q); - move_group_arm.setPoseTarget(target_pose); - move_group_arm.move(); - - // Release - gripper_joint_values[0] = GRIPPER_OPEN; - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); - - // Return to home and vertical pose - move_group_arm.setNamedTarget("home"); - move_group_arm.move(); - move_group_arm.setNamedTarget("vertical"); - move_group_arm.move(); - - gripper_joint_values[0] = GRIPPER_DEFAULT; - move_group_gripper.setJointValueTarget(gripper_joint_values); - move_group_gripper.move(); + auto controller = std::make_shared(node_options); + + // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる + std::thread spin_thread([controller]() { + rclcpp::spin(controller); + }); + + controller->initializeMoveGroup(); + + // 初期姿勢 + controller->moveArmToNamedPose("vertical"); + controller->setGripperAngle(controller->GRIPPER_DEFAULT); + + // ピック準備 + controller->moveArmToNamedPose("home"); + controller->setGripperAngle(controller->GRIPPER_OPEN); + controller->moveArmToPose(controller->ABOVE_POSE); + controller->moveArmToPose(controller->PRE_AND_POST_GRASP_POSE); + + // ピック動作 + controller->moveArmToPose(controller->GRASP_POSE); + controller->setGripperAngle(controller->GRIPPER_CLOSE); + controller->moveArmToPose(controller->PRE_AND_POST_GRASP_POSE); + + // プレース準備 + controller->moveArmToNamedPose("home"); + + // プレース動作 + controller->moveArmToPose(controller->RELEASE_POSE); + controller->setGripperAngle(controller->GRIPPER_OPEN); + + // 終了動作 + controller->moveArmToNamedPose("home"); + controller->moveArmToNamedPose("vertical"); + controller->setGripperAngle(controller->GRIPPER_DEFAULT); + + // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + return 0; } diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp index f62dd583..09d481cc 100644 --- a/crane_plus_examples/src/pick_and_place_tf.cpp +++ b/crane_plus_examples/src/pick_and_place_tf.cpp @@ -35,6 +35,7 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" #include "std_msgs/msg/string.hpp" + using namespace std::chrono_literals; using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; @@ -46,7 +47,7 @@ class PickAndPlaceTf : public rclcpp::Node rclcpp::Node::SharedPtr move_group_gripper_node) : Node("pick_and_place_tf_node") { - using namespace std::placeholders; + // アームおよびグリッパ制御用のMoveGroupInterfaceの初期化とパラメータ設定 move_group_arm_ = std::make_shared(move_group_arm_node, "arm_tcp"); move_group_arm_->setMaxVelocityScalingFactor(1.0); move_group_arm_->setMaxAccelerationScalingFactor(1.0); @@ -59,11 +60,11 @@ class PickAndPlaceTf : public rclcpp::Node move_group_arm_->setGoalPositionTolerance(1e-5); move_group_arm_->setGoalOrientationTolerance(1e-4); - // SRDFに定義されている"home"の姿勢にする + // 初期姿勢としてSRDFに定義されている 'home' の位置姿勢に移動 move_group_arm_->setNamedTarget("home"); move_group_arm_->move(); - // 可動範囲を制限する + // アームの可動範囲制限（Path Constraints）を設定して、関節の動きを制限する moveit_msgs::msg::Constraints constraints; constraints.name = "arm_constraints"; @@ -84,14 +85,16 @@ class PickAndPlaceTf : public rclcpp::Node move_group_arm_->setPathConstraints(constraints); - // 待機姿勢 - control_arm(0.0, 0.0, 0.3, 0, 0, 0); + // 初期位置としての待機姿勢に移動 + moveArmToPose(0.0, 0.0, 0.3, 0, 0, 0); + // TFの受信に必要なBufferとListenerを初期化 tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); + // 定期的にTFを監視してピッキングをトリガーするためのタイマーを設定（0.5秒周期） timer_ = this->create_wall_timer( 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); } @@ -99,7 +102,7 @@ class PickAndPlaceTf : public rclcpp::Node private: void on_timer() { - // target_0のtf位置姿勢を取得 + // target_0（把持対象）のTFを取得 geometry_msgs::msg::TransformStamped tf_msg; try { @@ -122,16 +125,18 @@ class PickAndPlaceTf : public rclcpp::Node const auto TF_ELAPSED_TIME = now.nanoseconds() - tf.stamp_.time_since_epoch().count(); const auto TF_STOP_TIME = now.nanoseconds() - tf_past_.stamp_.time_since_epoch().count(); - // 現在時刻から2秒以内に受け取ったtfを使用 + // 一定時間（2秒）以内に更新された有効なTFのみ処理する if (TF_ELAPSED_TIME < FILTERING_TIME.count()) { double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length(); - // 把持対象の位置が停止していることを判定 + + // 前回位置からの移動量が閾値未満（静止状態）であるかを判定 if (tf_diff < DISTANCE_THRESHOLD) { - // 把持対象が3秒以上停止している場合ピッキング動作開始 + // 一定時間（3秒）以上静止し続けている場合、ピッキング動作を開始 if (TF_STOP_TIME > STOP_TIME_THRESHOLD.count()) { picking(tf.getOrigin()); } } else { + // 移動した場合は過去のTF位置情報を更新 tf_past_ = tf; } } @@ -139,56 +144,40 @@ class PickAndPlaceTf : public rclcpp::Node void picking(tf2::Vector3 target_position) { - const double GRIPPER_DEFAULT = 0.0; - const double GRIPPER_OPEN = angles::from_degrees(-30.0); - const double GRIPPER_CLOSE = angles::from_degrees(10.0); + // 1. 掴み動作の準備とターゲットへの正対 + setGripperAngle(GRIPPER_OPEN_); - // 何かを掴んでいた時のためにハンドを開く - control_gripper(GRIPPER_OPEN); - - // ロボット座標系（2D）の原点から見た把持対象物への角度を計算 double x = target_position.x(); double y = target_position.y(); double theta_rad = std::atan2(y, x); double theta_deg = theta_rad * 180.0 / 3.1415926535; - // 把持対象物に正対する - control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg); + moveArmToPose(0.0, 0.0, 0.3, 0, 0, theta_deg); - // 掴みに行く - if (!control_arm(x, y, 0.04, 0, 90, theta_deg)) { - // アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る - control_arm(0.0, 0.0, 0.3, 0, 0, 0); + // 2. ターゲット位置へのアプローチと掴み動作 + if (!moveArmToPose(x, y, 0.04, 0, 90, theta_deg)) { + // アーム動作に失敗した場合は初期姿勢に戻る + moveArmToPose(0.0, 0.0, 0.3, 0, 0, 0); return; } - // ハンドを閉じる - control_gripper(GRIPPER_CLOSE); - - // 移動する - control_arm(0.12, 0.0, 0.17, 0, 90, 0); + setGripperAngle(GRIPPER_CLOSE_); - // 横を向く - control_arm(0.0, -0.12, 0.17, 0, 90, -90); + // 3. 搬送および配置動作 + moveArmToPose(0.12, 0.0, 0.17, 0, 90, 0); + moveArmToPose(0.0, -0.12, 0.17, 0, 90, -90); + moveArmToPose(0.0, -0.25, 0.05, 0, 90, -90); - // 下ろす - control_arm(0.0, -0.25, 0.05, 0, 90, -90); + setGripperAngle(GRIPPER_OPEN_); - // ハンドを開く - control_gripper(GRIPPER_OPEN); - - // 少しだけハンドを持ち上げる - control_arm(0.0, -0.25, 0.10, 0, 90, -90); - - // 待機姿勢に戻る - control_arm(0.0, 0.0, 0.3, 0, 0, 0); - - // ハンドを閉じる - control_gripper(GRIPPER_DEFAULT); + // 4. 待機姿勢への復帰 + moveArmToPose(0.0, -0.25, 0.10, 0, 90, -90); + moveArmToPose(0.0, 0.0, 0.3, 0, 0, 0); + setGripperAngle(GRIPPER_DEFAULT_); } - // グリッパ制御 - void control_gripper(const double angle) + // グリッパの開閉角度を設定して動かす + void setGripperAngle(const double angle) { auto joint_values = move_group_gripper_->getCurrentJointValues(); joint_values[0] = angle; @@ -196,8 +185,8 @@ class PickAndPlaceTf : public rclcpp::Node move_group_gripper_->move(); } - // アーム制御 - bool control_arm( + // x, y, z[m]とroll, pitch, yaw[deg]で指定した位置姿勢にアームを動かす + bool moveArmToPose( const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { @@ -220,6 +209,11 @@ class PickAndPlaceTf : public rclcpp::Node std::shared_ptr tf_listener_{nullptr}; rclcpp::TimerBase::SharedPtr timer_{nullptr}; tf2::Stamped tf_past_; + + // グリッパ角度の定数 + const double GRIPPER_DEFAULT_ = 0.0; + const double GRIPPER_OPEN_ = angles::from_degrees(-30.0); + const double GRIPPER_CLOSE_ = angles::from_degrees(10.0); }; int main(int argc, char ** argv) diff --git a/crane_plus_examples/src/pose_groupstate.cpp b/crane_plus_examples/src/pose_groupstate.cpp index e5d4c799..c72fcbee 100644 --- a/crane_plus_examples/src/pose_groupstate.cpp +++ b/crane_plus_examples/src/pose_groupstate.cpp @@ -16,37 +16,65 @@ // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes // /run_move_group/src/run_move_group.cpp +#include + #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -static const rclcpp::Logger LOGGER = rclcpp::get_logger("pose_groupstate"); +class PoseGroupstateController : public rclcpp::Node +{ +public: + explicit PoseGroupstateController(const rclcpp::NodeOptions & node_options) + : Node("pose_groupstate", node_options) + { + } + + // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す + void initializeMoveGroup() + { + move_group_arm_ = std::make_shared(shared_from_this(), "arm_tcp"); + move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + } + + // SRDFに定義されている名前付きの姿勢に移動する + void moveArmToNamedPose(const std::string & name) + { + move_group_arm_->setNamedTarget(name); + move_group_arm_->move(); + } + +private: + std::shared_ptr move_group_arm_; +}; int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); - // For current state monitor - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(move_group_arm_node); - std::thread([&executor]() {executor.spin();}).detach(); - MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 - move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 + auto arm_controller = std::make_shared(node_options); - move_group_arm.setNamedTarget("home"); - move_group_arm.move(); + // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる + std::thread spin_thread([arm_controller]() { + rclcpp::spin(arm_controller); + }); - move_group_arm.setNamedTarget("vertical"); - move_group_arm.move(); + arm_controller->initializeMoveGroup(); - move_group_arm.setNamedTarget("home"); - move_group_arm.move(); + // SRDFに定義されている名前付き姿勢を順番に実行する + arm_controller->moveArmToNamedPose("home"); + arm_controller->moveArmToNamedPose("vertical"); + arm_controller->moveArmToNamedPose("home"); + // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } + return 0; } diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 38867001..c99a13be 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -26,64 +26,47 @@ from rclpy.logging import get_logger +class GripperController: + def __init__(self): + # MoveItPyのインスタンスを生成し、planning componentを取得 + self.crane_plus = MoveItPy(node_name='gripper_control') + self.gripper = self.crane_plus.get_planning_component('gripper') + self.robot_model = self.crane_plus.get_robot_model() + + # プランニングパラメータの設定 + self.plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + self.logger = get_logger('gripper_control') + + def set_gripper_angle(self, angle): + # グリッパの目標角度を設定して動作計画・実行する + self.gripper.set_start_state_to_current_state() + robot_state = RobotState(self.robot_model) + robot_state.set_joint_group_positions('gripper', [angle]) + self.gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + self.crane_plus, + self.gripper, + self.logger, + single_plan_parameters=self.plan_request_params, + ) + + def main(args=None): rclpy.init(args=args) - logger = get_logger('gripper_control') - - # instantiate MoveItPy instance and get planning component - crane_plus = MoveItPy(node_name='gripper_control') - logger.info('MoveItPy instance created') - - # グリッパ制御用 planning component - gripper = crane_plus.get_planning_component('gripper') - - # instantiate a RobotState instance using the current robot model - robot_model = crane_plus.get_robot_model() - - plan_request_params = PlanRequestParameters( - crane_plus, - 'ompl_rrtc', - ) - - # 動作速度の調整 - plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - - # gripperを閉じる - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [math.radians(30.0)]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=plan_request_params, - ) - - # gripperを開く - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [math.radians(-30.0)]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=plan_request_params, - ) - - # gripperを0度にする - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [math.radians(0.0)]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=plan_request_params, - ) + + controller = GripperController() + + # グリッパを閉じる + controller.set_gripper_angle(math.radians(30.0)) + + # グリッパを開く + controller.set_gripper_angle(math.radians(-30.0)) + + # グリッパを0度にする + controller.set_gripper_angle(math.radians(0.0)) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 50eb7345..57d87b3d 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -27,35 +27,56 @@ from rclpy.logging import get_logger -def main(args=None): - rclpy.init(args=args) - logger = get_logger('joint_values') - - # instantiate MoveItPy instance and get planning component - crane_plus = MoveItPy(node_name='joint_values') - logger.info('MoveItPy instance created') +class JointValuesController: + def __init__(self): + # MoveItPyのインスタンスを生成し、planning componentを取得 + self.crane_plus = MoveItPy(node_name='joint_values') + self.arm = self.crane_plus.get_planning_component('arm_tcp') + self.robot_model = self.crane_plus.get_robot_model() + self.robot_state = RobotState(self.robot_model) + + # プランニングパラメータの設定 + self.plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + self.logger = get_logger('joint_values') + + def move_arm_to_named_pose(self, name): + # SRDFに定義されている名前付きの姿勢に移動する + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(configuration_name=name) + plan_and_execute( + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.plan_request_params, + ) - # アーム制御用 planning component - arm = crane_plus.get_planning_component('arm_tcp') + def move_arm_to_joint_values(self, joint_values_dict): + # 関節角度の辞書を指定してアームを動かす + self.robot_state.joint_positions = joint_values_dict + joint_constraint = construct_joint_constraint( + robot_state=self.robot_state, + joint_model_group=self.crane_plus.get_robot_model().get_joint_model_group('arm_tcp'), + ) + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + plan_and_execute( + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.plan_request_params, + ) - # instantiate a RobotState instance using the current robot model - robot_model = crane_plus.get_robot_model() - robot_state = RobotState(robot_model) + def get_current_joint_values(self, joint_names): + # 現在の関節角度を名前付き辞書で取得する + self.arm.set_start_state_to_current_state() + current_state = self.arm.get_start_state() + positions = current_state.get_joint_group_positions('arm_tcp') + return dict(zip(joint_names, positions)) - plan_request_params = PlanRequestParameters( - crane_plus, - 'ompl_rrtc', - ) - # 動作速度の調整 - # Set 0.0 ~ 1.0 - plan_request_params.max_acceleration_scaling_factor = 1.0 - plan_request_params.max_velocity_scaling_factor = 1.0 +def main(args=None): + rclpy.init(args=args) - # SRDF内に定義されている'vertical'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='vertical') - plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + controller = JointValuesController() joint_names = [ 'crane_plus_joint1', @@ -63,35 +84,19 @@ def main(args=None): 'crane_plus_joint3', 'crane_plus_joint4', ] - target_joint_value = math.radians(45) - - # 現在角度をベースに、目標角度を作成する - current_state = arm.get_start_state() - joint_values = current_state.get_joint_group_positions('arm_tcp') + target_joint_value = math.radians(45.0) - # jointのリストを辞書型の形式に変換する - joint_values_dict = dict(zip(joint_names, joint_values)) + # verticalの姿勢から開始 + controller.move_arm_to_named_pose('vertical') - # 各関節角度を順番に-45[deg]に動かす + # 各関節を順番に45度に動かす + joint_values_dict = controller.get_current_joint_values(joint_names) for joint_name in joint_names: - # 対象のjointに目標値を設定する joint_values_dict[joint_name] = target_joint_value - robot_state.joint_positions = joint_values_dict - - joint_constraint = construct_joint_constraint( - robot_state=robot_state, - joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm_tcp'), - ) - - arm.set_start_state_to_current_state() - arm.set_goal_state(motion_plan_constraints=[joint_constraint]) - - plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + controller.move_arm_to_joint_values(joint_values_dict) - # SRDF内に定義されている'vertical'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='vertical') - plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + # verticalの姿勢に戻る + controller.move_arm_to_named_pose('vertical') # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index ec331674..2a6739eb 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -32,270 +32,149 @@ from shape_msgs.msg import SolidPrimitive -def set_goal_constraints(pose): - # 位置姿勢の許容誤差 - POSITION_TOLERANCE = 0.00001 - ORIENTATION_TOLERANCE = 0.0001 - - # 目標位置姿勢 - target_pose = PoseStamped() - target_pose.header.frame_id = 'crane_plus_base' - target_pose.pose = pose - - # 目標位置姿勢の制約設定 - goal_constraints = Constraints() - goal_constraints.name = 'tolerance_goal' - - # 位置の制約設定 - position_constraint = PositionConstraint() - position_constraint.header.frame_id = 'crane_plus_base' - position_constraint.link_name = 'crane_plus_link_tcp' - tolerance_region = BoundingVolume() - primitive = SolidPrimitive() - primitive.type = SolidPrimitive.SPHERE - primitive.dimensions = [POSITION_TOLERANCE] - tolerance_region.primitives.append(primitive) - tolerance_region.primitive_poses.append(target_pose.pose) - position_constraint.constraint_region = tolerance_region - position_constraint.weight = 1.0 - - # 姿勢の制約設定 - orientation_constraint = OrientationConstraint() - orientation_constraint.header.frame_id = 'crane_plus_base' - orientation_constraint.link_name = 'crane_plus_link_tcp' - orientation_constraint.orientation = target_pose.pose.orientation - orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE - orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE - orientation_constraint.absolute_z_axis_tolerance = ORIENTATION_TOLERANCE - orientation_constraint.weight = 1.0 - - goal_constraints.position_constraints.append(position_constraint) - goal_constraints.orientation_constraints.append(orientation_constraint) - - # 制約設定を返す - return goal_constraints +class PickAndPlaceController: + def __init__(self): + # MoveItPyのインスタンスを生成し、planning componentを取得 + self.crane_plus = MoveItPy(node_name='pick_and_place') + self.arm = self.crane_plus.get_planning_component('arm_tcp') + self.gripper = self.crane_plus.get_planning_component('gripper') + self.robot_model = self.crane_plus.get_robot_model() + + # プランニングパラメータの設定 + self.arm_plan_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.arm_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.arm_plan_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + self.gripper_plan_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.gripper_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.gripper_plan_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + self.logger = get_logger('pick_and_place') + + # グリッパの開閉角 + self.GRIPPER_DEFAULT = 0.0 + self.GRIPPER_OPEN = math.radians(-30.0) + self.GRIPPER_CLOSE = math.radians(10.0) + + # 物体を持ち上げる高さ + LIFTING_HEIGHT = 0.03 + # 目標位置姿勢の定数 + self.ABOVE_POSE = self._make_pose(0.0, -0.21, 0.17, 0.0, 90.0, -90.0) + self.GRASP_POSE = self._make_pose(0.0, -0.1, 0.02, 0.0, 180.0, -90.0) + pre_post = copy.deepcopy(self.GRASP_POSE) + pre_post.position.z = LIFTING_HEIGHT + self.PRE_AND_POST_GRASP_POSE = pre_post + self.RELEASE_POSE = self._make_pose(0.25, 0.0, 0.06, 0.0, 90.0, 0.0) + + @staticmethod + def _make_pose(x, y, z, roll, pitch, yaw): + # x, y, z[m]とroll, pitch, yaw[deg]からPoseを生成する + quat = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True).as_quat() + return Pose( + position=Point(x=x, y=y, z=z), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + + @staticmethod + def _make_goal_constraints(pose): + # 位置・姿勢の目標制約を生成する + POSITION_TOLERANCE = 0.00001 + ORIENTATION_TOLERANCE = 0.0001 + + target_pose = PoseStamped() + target_pose.header.frame_id = 'crane_plus_base' + target_pose.pose = pose + + goal_constraints = Constraints() + goal_constraints.name = 'tolerance_goal' + + position_constraint = PositionConstraint() + position_constraint.header.frame_id = 'crane_plus_base' + position_constraint.link_name = 'crane_plus_link_tcp' + tolerance_region = BoundingVolume() + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.SPHERE + primitive.dimensions = [POSITION_TOLERANCE] + tolerance_region.primitives.append(primitive) + tolerance_region.primitive_poses.append(target_pose.pose) + position_constraint.constraint_region = tolerance_region + position_constraint.weight = 1.0 + + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = 'crane_plus_base' + orientation_constraint.link_name = 'crane_plus_link_tcp' + orientation_constraint.orientation = target_pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_z_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.weight = 1.0 + + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orientation_constraint) + return goal_constraints + + def move_arm_to_named_pose(self, name): + # SRDFに定義されている名前付きの姿勢に移動する + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(configuration_name=name) + plan_and_execute( + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.arm_plan_params, + ) + + def move_arm_to_pose(self, pose): + # 位置姿勢の制約を設定してアームを動かす + goal_constraints = self._make_goal_constraints(pose) + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.arm_plan_params, + ) + + def set_gripper_angle(self, angle): + # グリッパの目標角度を設定して動作計画・実行する + self.gripper.set_start_state_to_current_state() + robot_state = RobotState(self.robot_model) + robot_state.set_joint_group_positions('gripper', [angle]) + self.gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + self.crane_plus, self.gripper, self.logger, + single_plan_parameters=self.gripper_plan_params, + ) def main(args=None): rclpy.init(args=args) - logger = get_logger('pick_and_place') - - # instantiate MoveItPy instance and get planning component - crane_plus = MoveItPy(node_name='pick_and_place') - logger.info('MoveItPy instance created') - - # アーム制御用 planning component - arm = crane_plus.get_planning_component('arm_tcp') - # グリッパ制御用 planning component - gripper = crane_plus.get_planning_component('gripper') - - robot_model = crane_plus.get_robot_model() - - arm_plan_request_params = PlanRequestParameters( - crane_plus, - 'ompl_rrtc', - ) - gripper_plan_request_params = PlanRequestParameters( - crane_plus, - 'ompl_rrtc', - ) - - # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - - gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - - # グリッパの開閉角 - GRIPPER_DEFAULT = 0.0 - GRIPPER_OPEN = math.radians(-30.0) - GRIPPER_CLOSE = math.radians(10.0) - # 物体を持ち上げる高さ - LIFTING_HEIGHT = 0.03 - # 物体の頭上の位置 - gripper_quat = Rotation.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True).as_quat() - gripper_quat_msg = Quaternion( - x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3] - ) - ABOVE_POSE = Pose(position=Point(x=0.0, y=-0.21, z=0.17), orientation=gripper_quat_msg) - # 物体を掴む位置 - gripper_quat = Rotation.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True).as_quat() - gripper_quat_msg = Quaternion( - x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3] - ) - GRASP_POSE = Pose(position=Point(x=0.0, y=-0.1, z=0.02), orientation=gripper_quat_msg) - PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) - PRE_AND_POST_GRASP_POSE.position.z = LIFTING_HEIGHT - # 物体を置く位置 - gripper_quat = Rotation.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True).as_quat() - gripper_quat_msg = Quaternion( - x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3] - ) - RELEASE_POSE = Pose(position=Point(x=0.25, y=0.0, z=0.06), orientation=gripper_quat_msg) - - # SRDF内に定義されている'vertical'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='vertical') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # gripperの開閉角を0度にする - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # SRDF内に定義されている'home'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='home') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # gripperを開く - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # 物体の上に腕を伸ばす - arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(ABOVE_POSE) - arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(PRE_AND_POST_GRASP_POSE) - arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # 掴みに行く - arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(GRASP_POSE) - arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # ハンドを閉じる - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # 持ち上げる - arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(PRE_AND_POST_GRASP_POSE) - arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # SRDF内に定義されている'home'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='home') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # 下ろす - arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(RELEASE_POSE) - arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # ハンドを開く - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # SRDF内に定義されている'home'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='home') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # SRDF内に定義されている'vertical'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='vertical') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # gripperの開閉角を0度にする - gripper.set_start_state_to_current_state() - robot_state = RobotState(robot_model) - robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) - gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) + + controller = PickAndPlaceController() + + # 初期姿勢 + controller.move_arm_to_named_pose('vertical') + controller.set_gripper_angle(controller.GRIPPER_DEFAULT) + + # ピック準備 + controller.move_arm_to_named_pose('home') + controller.set_gripper_angle(controller.GRIPPER_OPEN) + controller.move_arm_to_pose(controller.ABOVE_POSE) + controller.move_arm_to_pose(controller.PRE_AND_POST_GRASP_POSE) + + # ピック動作 + controller.move_arm_to_pose(controller.GRASP_POSE) + controller.set_gripper_angle(controller.GRIPPER_CLOSE) + controller.move_arm_to_pose(controller.PRE_AND_POST_GRASP_POSE) + + # プレース準備 + controller.move_arm_to_named_pose('home') + + # プレース動作 + controller.move_arm_to_pose(controller.RELEASE_POSE) + controller.set_gripper_angle(controller.GRIPPER_OPEN) + + # 終了動作 + controller.move_arm_to_named_pose('home') + controller.move_arm_to_named_pose('vertical') + controller.set_gripper_angle(controller.GRIPPER_DEFAULT) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 6dd60bed..1a544490 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -46,41 +46,39 @@ def __init__(self): super().__init__('pick_and_place_tf') self.logger = self.get_logger() - # tf + # TFの受信に必要なBufferとListenerを初期化 self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.tf_past = TransformStamped() - # instantiate MoveItPy instance and get planning component + # MoveItPyのインスタンスを生成し、planning componentを取得 self.crane_plus = MoveItPy(node_name='moveit_py') self.logger.info('MoveItPy instance created') - # アーム制御用 planning component + # アームおよびグリッパ制御用のplanning componentを取得 self.arm = self.crane_plus.get_planning_component('arm_tcp') - # グリッパ制御用 planning component self.gripper = self.crane_plus.get_planning_component('gripper') - # instantiate a RobotState instance using the current robot model + # ロボットモデルからRobotStateインスタンスを取得（グリッパの関節角設定等に使用） self.robot_model = self.crane_plus.get_robot_model() - self.arm_plan_request_params = PlanRequestParameters( - self.crane_plus, - 'ompl_rrtc', - ) - self.gripper_plan_request_params = PlanRequestParameters( - self.crane_plus, - 'ompl_rrtc', - ) + # 計画時のパラメータ（PlanRequestParameters）を設定 + self.arm_plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.gripper_plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - # 動作速度の調整 - # Set 0.0 ~ 1.0 + # アームとグリッパの最大速度・加速度スケーリングファクタを設定 (0.0 ~ 1.0) self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 - # SRDFに定義されている'home'の姿勢にする + # グリッパの目標開閉角度を設定 + self.GRIPPER_DEFAULT = 0.0 + self.GRIPPER_OPEN = math.radians(-30.0) + self.GRIPPER_CLOSE = math.radians(10.0) + + # 初期姿勢としてSRDFに定義されている 'home' の位置姿勢に移動 self.arm.set_start_state_to_current_state() self.arm.set_goal_state(configuration_name='home') plan_and_execute( @@ -90,35 +88,36 @@ def __init__(self): single_plan_parameters=self.arm_plan_request_params, ) - # 可動範囲を制限する + # アームの可動範囲制限（Path Constraints）を設定して、関節の動きを制限する constraints = Constraints() constraints.name = 'arm_constraints' - jointConstraint = JointConstraint() - jointConstraint.joint_name = 'crane_plus_joint1' - jointConstraint.position = 0.0 - jointConstraint.tolerance_above = math.radians(100) - jointConstraint.tolerance_below = math.radians(100) - jointConstraint.weight = 1.0 - constraints.joint_constraints.append(jointConstraint) - - jointConstraint.joint_name = 'crane_plus_joint3' - jointConstraint.position = 0.0 - jointConstraint.tolerance_above = math.radians(0) - jointConstraint.tolerance_below = math.radians(180) - jointConstraint.weight = 1.0 - constraints.joint_constraints.append(jointConstraint) + joint_constraint = JointConstraint() + joint_constraint.joint_name = 'crane_plus_joint1' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(100) + joint_constraint.tolerance_below = math.radians(100) + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + joint_constraint = JointConstraint() + joint_constraint.joint_name = 'crane_plus_joint3' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(0) + joint_constraint.tolerance_below = math.radians(180) + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) self.arm.set_path_constraints(constraints) - # 待機姿勢 - self._control_arm(0.0, 0.0, 0.3, 0, 0, 0.0) + # 初期位置としての待機姿勢に移動 + self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, 0.0) - # Call on_timer function every second + # 定期的にTFを監視してピッキングをトリガーするためのタイマーを設定（0.5秒周期） self.timer = self.create_timer(0.5, self.on_timer) def on_timer(self): - # target_0のtf位置姿勢を取得 + # target_0（把持対象）のTFを取得 try: tf_msg = self.tf_buffer.lookup_transform( 'crane_plus_base', 'target_0', rclpy.time.Time() @@ -132,15 +131,13 @@ def on_timer(self): STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3) DISTANCE_THRESHOLD = 0.01 - # 経過時間と停止時間を計算(nsec) - # 経過時間 + # ターゲットのTFが更新された時刻からの経過時間と、前回の位置から静止している時間を計算 tf_time = rclpy.time.Time.from_msg(tf_msg.header.stamp) TF_ELAPSED_TIME = now_time - tf_time - # 停止時間 tf_past_time = rclpy.time.Time.from_msg(self.tf_past.header.stamp) TF_STOP_TIME = now_time - tf_past_time - # 現在時刻から2秒以内に受け取ったtfを使用 + # 一定時間（2秒）以内に更新された有効なTFのみ処理する if TF_ELAPSED_TIME < FILTERING_TIME: tf_diff = np.linalg.norm( [ @@ -149,63 +146,49 @@ def on_timer(self): self.tf_past.transform.translation.z - tf_msg.transform.translation.z, ] ) - # 把持対象の位置が停止していることを判定 + + # 前回位置からの移動量が閾値未満（静止状態）であるかを判定 if tf_diff < DISTANCE_THRESHOLD: - # 把持対象が3秒以上停止している場合ピッキング動作開始 + # 一定時間（3秒）以上静止し続けている場合、ピッキング動作を開始 if TF_STOP_TIME > STOP_TIME_THRESHOLD: - self._picking(tf_msg.transform.translation) + self.picking(tf_msg.transform.translation) else: + # 移動した場合は過去のTF位置情報を更新 self.tf_past = tf_msg - def _picking(self, target_position): - GRIPPER_DEFAULT = 0.0 - GRIPPER_OPEN = math.radians(-30.0) - GRIPPER_CLOSE = math.radians(10.0) + def picking(self, target_position): + # 1. 掴み動作の準備とターゲットへの正対 + self.set_gripper_angle(self.GRIPPER_OPEN) - # 何かを掴んでいた時のためにハンドを開く - self._control_gripper(GRIPPER_OPEN) - - # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 x = target_position.x y = target_position.y theta_rad = math.atan2(y, x) theta_deg = math.degrees(theta_rad) - # 把持対象物に正対する - self._control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg) + self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, theta_deg) - # 掴みに行く - if not self._control_arm(x, y, 0.04, 0, 90, theta_deg): - # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る - self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) + # 2. ターゲット位置へのアプローチと掴み動作 + if not self.move_arm_to_pose(x, y, 0.04, 0, 90, theta_deg): + # アーム動作に失敗した場合は初期姿勢に戻る + self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, 0) return - # ハンドを閉じる - self._control_gripper(GRIPPER_CLOSE) - - # 移動する - self._control_arm(0.12, 0.0, 0.17, 0, 90, 0) - - # 横へ移動する - self._control_arm(0.0, -0.12, 0.17, 0, 90, -90) + self.set_gripper_angle(self.GRIPPER_CLOSE) - # 下ろす - self._control_arm(0.0, -0.25, 0.05, 0, 90, -90) + # 3. 搬送および配置動作 + self.move_arm_to_pose(0.12, 0.0, 0.17, 0, 90, 0) + self.move_arm_to_pose(0.0, -0.12, 0.17, 0, 90, -90) + self.move_arm_to_pose(0.0, -0.25, 0.05, 0, 90, -90) - # ハンドを開く - self._control_gripper(GRIPPER_OPEN) + self.set_gripper_angle(self.GRIPPER_OPEN) - # 少しだけハンドを持ち上げる - self._control_arm(0.0, -0.25, 0.10, 0, 90, -90) + # 4. 待機姿勢への復帰 + self.move_arm_to_pose(0.0, -0.25, 0.10, 0, 90, -90) + self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, 0) + self.set_gripper_angle(self.GRIPPER_DEFAULT) - # 待機姿勢に戻る - self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) - - # ハンドを閉じる - self._control_gripper(GRIPPER_DEFAULT) - - # グリッパ制御 - def _control_gripper(self, angle): + def set_gripper_angle(self, angle): + # グリッパの目標関節角度を設定して動作計画・実行する self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) @@ -217,13 +200,12 @@ def _control_gripper(self, angle): single_plan_parameters=self.gripper_plan_request_params, ) - # アーム制御 - def _control_arm(self, x, y, z, roll, pitch, yaw): - # 位置姿勢の許容誤差 + def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): + # 指定された位置姿勢（x, y, z [m]、roll, pitch, yaw [deg]）へアームを移動する POSITION_TOLERANCE = 0.00001 ORIENTATION_TOLERANCE = 0.0001 - # 目標位置姿勢 + # 目標のPoseStampedオブジェクトを生成 target_pose = PoseStamped() target_pose.header.frame_id = 'crane_plus_base' target_pose.pose.position.x = x @@ -236,11 +218,11 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose.pose.orientation.z = quat[2] target_pose.pose.orientation.w = quat[3] - # 目標位置姿勢の制約設定 + # 目標位置・姿勢の制約（Constraints）を設定 goal_constraints = Constraints() goal_constraints.name = 'tolerance_goal' - # 位置の制約設定 + # 位置制約を設定 position_constraint = PositionConstraint() position_constraint.header.frame_id = 'crane_plus_base' position_constraint.link_name = 'crane_plus_link_tcp' @@ -253,7 +235,7 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): position_constraint.constraint_region = tolerance_region position_constraint.weight = 1.0 - # 姿勢の制約設定 + # 姿勢制約を設定 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = 'crane_plus_base' orientation_constraint.link_name = 'crane_plus_link_tcp' @@ -268,17 +250,17 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): self.arm.set_start_state_to_current_state() self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - result = plan_and_execute( + + # 計画および実行 + return plan_and_execute( self.crane_plus, self.arm, self.logger, single_plan_parameters=self.arm_plan_request_params, ) - return result def main(args=None): - # ros2の初期化 rclpy.init(args=args) pick_and_place_tf_node = PickAndPlaceTf() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index cef4555c..478bd77d 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -23,55 +23,40 @@ from rclpy.logging import get_logger -def main(args=None): - rclpy.init(args=args) - logger = get_logger('pose_groupstate') +class PoseGroupstateController: + def __init__(self): + # MoveItPyのインスタンスを生成し、planning componentを取得 + self.crane_plus = MoveItPy(node_name='pose_groupstate') + self.arm = self.crane_plus.get_planning_component('arm_tcp') - # instantiate MoveItPy instance and get planning component - crane_plus = MoveItPy(node_name='pose_groupstate') - logger.info('MoveItPy instance created') + # プランニングパラメータの設定 + self.plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # アーム制御用 planning component - arm = crane_plus.get_planning_component('arm_tcp') + self.logger = get_logger('pose_groupstate') - plan_request_params = PlanRequestParameters( - crane_plus, - 'ompl_rrtc', - ) + def move_arm_to_named_pose(self, name): + # SRDFに定義されている名前付きの姿勢に移動する + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(configuration_name=name) + plan_and_execute( + self.crane_plus, + self.arm, + self.logger, + single_plan_parameters=self.plan_request_params, + ) - # 動作速度の調整 - plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # SRDF内に定義されている'home'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='home') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params, - ) +def main(args=None): + rclpy.init(args=args) - # SRDF内に定義されている'vertical'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='vertical') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params, - ) + controller = PoseGroupstateController() - # SRDF内に定義されている'home'の姿勢にする - arm.set_start_state_to_current_state() - arm.set_goal_state(configuration_name='home') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params, - ) + # SRDFに定義されている名前付き姿勢を順番に実行する + controller.move_arm_to_named_pose('home') + controller.move_arm_to_named_pose('vertical') + controller.move_arm_to_named_pose('home') # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 From e44c6bde412bc63c4d0d6654d7ef51977413495b Mon Sep 17 00:00:00 2001 From: motty Date: Tue, 30 Jun 2026 11:07:37 +0900 Subject: [PATCH 2/8] set to move --- crane_plus_examples/src/gripper_control.cpp | 8 ++++---- .../crane_plus_examples_py/gripper_control.py | 9 ++++----- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp index d6245228..c5ae188b 100644 --- a/crane_plus_examples/src/gripper_control.cpp +++ b/crane_plus_examples/src/gripper_control.cpp @@ -41,7 +41,7 @@ class GripperController : public rclcpp::Node } // グリッパの開閉角度を設定して動かす - void setGripperAngle(const double angle) + void moveGripperAngle(const double angle) { auto joint_values = move_group_gripper_->getCurrentJointValues(); joint_values[0] = angle; @@ -74,13 +74,13 @@ int main(int argc, char ** argv) gripper_controller->initializeMoveGroup(); // グリッパを閉じる - gripper_controller->setGripperAngle(toRadians(30.0)); + gripper_controller->moveGripperAngle(toRadians(30.0)); // グリッパを開く - gripper_controller->setGripperAngle(toRadians(-30.0)); + gripper_controller->moveGripperAngle(toRadians(-30.0)); // グリッパを0度にする - gripper_controller->setGripperAngle(toRadians(0.0)); + gripper_controller->moveGripperAngle(toRadians(0.0)); // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index c99a13be..7b82bc18 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -40,8 +40,7 @@ def __init__(self): self.logger = get_logger('gripper_control') - def set_gripper_angle(self, angle): - # グリッパの目標角度を設定して動作計画・実行する + def move_gripper_angle(self, angle): self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) @@ -60,13 +59,13 @@ def main(args=None): controller = GripperController() # グリッパを閉じる - controller.set_gripper_angle(math.radians(30.0)) + controller.move_gripper_angle(math.radians(30.0)) # グリッパを開く - controller.set_gripper_angle(math.radians(-30.0)) + controller.move_gripper_angle(math.radians(-30.0)) # グリッパを0度にする - controller.set_gripper_angle(math.radians(0.0)) + controller.move_gripper_angle(math.radians(0.0)) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 From 8290e05e095b3ba0370a1d45531dd37e8ff2422c Mon Sep 17 00:00:00 2001 From: motty Date: Tue, 30 Jun 2026 11:09:55 +0900 Subject: [PATCH 3/8] set to move --- crane_plus_examples/src/pick_and_place.cpp | 12 ++++++------ .../crane_plus_examples_py/pick_and_place.py | 13 ++++++------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp index 7bf83d2a..1d7b8c42 100644 --- a/crane_plus_examples/src/pick_and_place.cpp +++ b/crane_plus_examples/src/pick_and_place.cpp @@ -78,7 +78,7 @@ class PickAndPlaceController : public rclcpp::Node } // グリッパの開閉角度を設定して動かす - void setGripperAngle(const double angle) + void moveGripperAngle(const double angle) { auto joint_values = move_group_gripper_->getCurrentJointValues(); joint_values[0] = angle; @@ -139,17 +139,17 @@ int main(int argc, char ** argv) // 初期姿勢 controller->moveArmToNamedPose("vertical"); - controller->setGripperAngle(controller->GRIPPER_DEFAULT); + controller->moveGripperAngle(controller->GRIPPER_DEFAULT); // ピック準備 controller->moveArmToNamedPose("home"); - controller->setGripperAngle(controller->GRIPPER_OPEN); + controller->moveGripperAngle(controller->GRIPPER_OPEN); controller->moveArmToPose(controller->ABOVE_POSE); controller->moveArmToPose(controller->PRE_AND_POST_GRASP_POSE); // ピック動作 controller->moveArmToPose(controller->GRASP_POSE); - controller->setGripperAngle(controller->GRIPPER_CLOSE); + controller->moveGripperAngle(controller->GRIPPER_CLOSE); controller->moveArmToPose(controller->PRE_AND_POST_GRASP_POSE); // プレース準備 @@ -157,12 +157,12 @@ int main(int argc, char ** argv) // プレース動作 controller->moveArmToPose(controller->RELEASE_POSE); - controller->setGripperAngle(controller->GRIPPER_OPEN); + controller->moveGripperAngle(controller->GRIPPER_OPEN); // 終了動作 controller->moveArmToNamedPose("home"); controller->moveArmToNamedPose("vertical"); - controller->setGripperAngle(controller->GRIPPER_DEFAULT); + controller->moveGripperAngle(controller->GRIPPER_DEFAULT); // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 2a6739eb..96baa212 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -132,8 +132,7 @@ def move_arm_to_pose(self, pose): single_plan_parameters=self.arm_plan_params, ) - def set_gripper_angle(self, angle): - # グリッパの目標角度を設定して動作計画・実行する + def move_gripper_angle(self, angle): self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) @@ -151,17 +150,17 @@ def main(args=None): # 初期姿勢 controller.move_arm_to_named_pose('vertical') - controller.set_gripper_angle(controller.GRIPPER_DEFAULT) + controller.move_gripper_angle(controller.GRIPPER_DEFAULT) # ピック準備 controller.move_arm_to_named_pose('home') - controller.set_gripper_angle(controller.GRIPPER_OPEN) + controller.move_gripper_angle(controller.GRIPPER_OPEN) controller.move_arm_to_pose(controller.ABOVE_POSE) controller.move_arm_to_pose(controller.PRE_AND_POST_GRASP_POSE) # ピック動作 controller.move_arm_to_pose(controller.GRASP_POSE) - controller.set_gripper_angle(controller.GRIPPER_CLOSE) + controller.move_gripper_angle(controller.GRIPPER_CLOSE) controller.move_arm_to_pose(controller.PRE_AND_POST_GRASP_POSE) # プレース準備 @@ -169,12 +168,12 @@ def main(args=None): # プレース動作 controller.move_arm_to_pose(controller.RELEASE_POSE) - controller.set_gripper_angle(controller.GRIPPER_OPEN) + controller.move_gripper_angle(controller.GRIPPER_OPEN) # 終了動作 controller.move_arm_to_named_pose('home') controller.move_arm_to_named_pose('vertical') - controller.set_gripper_angle(controller.GRIPPER_DEFAULT) + controller.move_gripper_angle(controller.GRIPPER_DEFAULT) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 From 004ed46e0045018a62c7328fb88dc3fe503c0523 Mon Sep 17 00:00:00 2001 From: motty Date: Tue, 30 Jun 2026 11:25:06 +0900 Subject: [PATCH 4/8] =?UTF-8?q?move=20=E3=82=84=20=E5=A4=89=E6=95=B0?= =?UTF-8?q?=E5=8C=96=E3=81=AE=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples/src/pick_and_place_tf.cpp | 57 ++++++++++++++----- .../pick_and_place_tf.py | 41 ++++++------- 2 files changed, 65 insertions(+), 33 deletions(-) diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp index 09d481cc..a6e2363b 100644 --- a/crane_plus_examples/src/pick_and_place_tf.cpp +++ b/crane_plus_examples/src/pick_and_place_tf.cpp @@ -86,7 +86,7 @@ class PickAndPlaceTf : public rclcpp::Node move_group_arm_->setPathConstraints(constraints); // 初期位置としての待機姿勢に移動 - moveArmToPose(0.0, 0.0, 0.3, 0, 0, 0); + moveArmToPose(STANDBY_POSITION_); // TFの受信に必要なBufferとListenerを初期化 tf_buffer_ = @@ -100,6 +100,16 @@ class PickAndPlaceTf : public rclcpp::Node } private: + // 位置姿勢のパラメータを格納する構造体 + struct PoseParams { + double x; + double y; + double z; + double roll; + double pitch; + double yaw; + }; + void on_timer() { // target_0（把持対象）のTFを取得 @@ -145,39 +155,39 @@ class PickAndPlaceTf : public rclcpp::Node void picking(tf2::Vector3 target_position) { // 1. 掴み動作の準備とターゲットへの正対 - setGripperAngle(GRIPPER_OPEN_); + moveGripperAngle(GRIPPER_OPEN_); double x = target_position.x(); double y = target_position.y(); double theta_rad = std::atan2(y, x); double theta_deg = theta_rad * 180.0 / 3.1415926535; - moveArmToPose(0.0, 0.0, 0.3, 0, 0, theta_deg); + moveArmToPose(0.0, 0.0, STANDBY_POSITION_.z, 0, 0, theta_deg); // 2. ターゲット位置へのアプローチと掴み動作 - if (!moveArmToPose(x, y, 0.04, 0, 90, theta_deg)) { + if (!moveArmToPose(x, y, GRASP_HEIGHT_, 0, GRASP_PITCH_, theta_deg)) { // アーム動作に失敗した場合は初期姿勢に戻る - moveArmToPose(0.0, 0.0, 0.3, 0, 0, 0); + moveArmToPose(STANDBY_POSITION_); return; } - setGripperAngle(GRIPPER_CLOSE_); + moveGripperAngle(GRIPPER_CLOSE_); // 3. 搬送および配置動作 - moveArmToPose(0.12, 0.0, 0.17, 0, 90, 0); - moveArmToPose(0.0, -0.12, 0.17, 0, 90, -90); - moveArmToPose(0.0, -0.25, 0.05, 0, 90, -90); + moveArmToPose(TRANSIT_POSE_1_); + moveArmToPose(TRANSIT_POSE_2_); + moveArmToPose(PLACE_POSE_); - setGripperAngle(GRIPPER_OPEN_); + moveGripperAngle(GRIPPER_OPEN_); // 4. 待機姿勢への復帰 - moveArmToPose(0.0, -0.25, 0.10, 0, 90, -90); - moveArmToPose(0.0, 0.0, 0.3, 0, 0, 0); - setGripperAngle(GRIPPER_DEFAULT_); + moveArmToPose(PLACE_RETRACT_POSE_); + moveArmToPose(STANDBY_POSITION_); + moveGripperAngle(GRIPPER_DEFAULT_); } // グリッパの開閉角度を設定して動かす - void setGripperAngle(const double angle) + void moveGripperAngle(const double angle) { auto joint_values = move_group_gripper_->getCurrentJointValues(); joint_values[0] = angle; @@ -185,6 +195,12 @@ class PickAndPlaceTf : public rclcpp::Node move_group_gripper_->move(); } + // 構造体で指定した位置姿勢にアームを動かす + bool moveArmToPose(const PoseParams & pose) + { + return moveArmToPose(pose.x, pose.y, pose.z, pose.roll, pose.pitch, pose.yaw); + } + // x, y, z[m]とroll, pitch, yaw[deg]で指定した位置姿勢にアームを動かす bool moveArmToPose( const double x, const double y, const double z, @@ -214,6 +230,19 @@ class PickAndPlaceTf : public rclcpp::Node const double GRIPPER_DEFAULT_ = 0.0; const double GRIPPER_OPEN_ = angles::from_degrees(-30.0); const double GRIPPER_CLOSE_ = angles::from_degrees(10.0); + + // 待機姿勢の位置姿勢（x, y, z [m], roll, pitch, yaw [deg]） + const PoseParams STANDBY_POSITION_ = {0.0, 0.0, 0.3, 0.0, 0.0, 0.0}; + + // 把持アプローチ時の高さおよびピッチ角 + const double GRASP_HEIGHT_ = 0.04; + const double GRASP_PITCH_ = 90.0; + + // 搬送時の中間姿勢およびプレース位置姿勢 + const PoseParams TRANSIT_POSE_1_ = {0.12, 0.0, 0.17, 0.0, 90.0, 0.0}; + const PoseParams TRANSIT_POSE_2_ = {0.0, -0.12, 0.17, 0.0, 90.0, -90.0}; + const PoseParams PLACE_POSE_ = {0.0, -0.25, 0.05, 0.0, 90.0, -90.0}; + const PoseParams PLACE_RETRACT_POSE_ = {0.0, -0.25, 0.10, 0.0, 90.0, -90.0}; }; int main(int argc, char ** argv) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 1a544490..f0817848 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -78,6 +78,15 @@ def __init__(self): self.GRIPPER_OPEN = math.radians(-30.0) self.GRIPPER_CLOSE = math.radians(10.0) + # 待機姿勢の位置姿勢（x, y, z [m], roll, pitch, yaw [deg]） + self.STANDBY_POSITION = (0.0, 0.0, 0.3, 0.0, 0.0, 0.0) + + # 搬送時の中間姿勢およびプレース位置姿勢 + self.TRANSIT_POSE_1 = (0.12, 0.0, 0.17, 0.0, 90.0, 0.0) + self.TRANSIT_POSE_2 = (0.0, -0.12, 0.17, 0.0, 90.0, -90.0) + self.PLACE_POSE = (0.0, -0.25, 0.05, 0.0, 90.0, -90.0) + self.PLACE_RETRACT_POSE = (0.0, -0.25, 0.10, 0.0, 90.0, -90.0) + # 初期姿勢としてSRDFに定義されている 'home' の位置姿勢に移動 self.arm.set_start_state_to_current_state() self.arm.set_goal_state(configuration_name='home') @@ -111,7 +120,7 @@ def __init__(self): self.arm.set_path_constraints(constraints) # 初期位置としての待機姿勢に移動 - self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, 0.0) + self.move_arm_to_pose(*self.STANDBY_POSITION) # 定期的にTFを監視してピッキングをトリガーするためのタイマーを設定（0.5秒周期） self.timer = self.create_timer(0.5, self.on_timer) @@ -158,7 +167,7 @@ def on_timer(self): def picking(self, target_position): # 1. 掴み動作の準備とターゲットへの正対 - self.set_gripper_angle(self.GRIPPER_OPEN) + self.move_gripper_angle(self.GRIPPER_OPEN) x = target_position.x y = target_position.y @@ -168,27 +177,26 @@ def picking(self, target_position): self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, theta_deg) # 2. ターゲット位置へのアプローチと掴み動作 - if not self.move_arm_to_pose(x, y, 0.04, 0, 90, theta_deg): + if not self.move_arm_to_pose(x, y, 0.04, 0, 90.0, theta_deg): # アーム動作に失敗した場合は初期姿勢に戻る - self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, 0) + self.move_arm_to_pose(*self.STANDBY_POSITION) return - self.set_gripper_angle(self.GRIPPER_CLOSE) + self.move_gripper_angle(self.GRIPPER_CLOSE) # 3. 搬送および配置動作 - self.move_arm_to_pose(0.12, 0.0, 0.17, 0, 90, 0) - self.move_arm_to_pose(0.0, -0.12, 0.17, 0, 90, -90) - self.move_arm_to_pose(0.0, -0.25, 0.05, 0, 90, -90) + self.move_arm_to_pose(*self.TRANSIT_POSE_1) + self.move_arm_to_pose(*self.TRANSIT_POSE_2) + self.move_arm_to_pose(*self.PLACE_POSE) - self.set_gripper_angle(self.GRIPPER_OPEN) + self.move_gripper_angle(self.GRIPPER_OPEN) # 4. 待機姿勢への復帰 - self.move_arm_to_pose(0.0, -0.25, 0.10, 0, 90, -90) - self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, 0) - self.set_gripper_angle(self.GRIPPER_DEFAULT) + self.move_arm_to_pose(*self.PLACE_RETRACT_POSE) + self.move_arm_to_pose(*self.STANDBY_POSITION) + self.move_gripper_angle(self.GRIPPER_DEFAULT) - def set_gripper_angle(self, angle): - # グリッパの目標関節角度を設定して動作計画・実行する + def move_gripper_angle(self, angle): self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) @@ -205,7 +213,6 @@ def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): POSITION_TOLERANCE = 0.00001 ORIENTATION_TOLERANCE = 0.0001 - # 目標のPoseStampedオブジェクトを生成 target_pose = PoseStamped() target_pose.header.frame_id = 'crane_plus_base' target_pose.pose.position.x = x @@ -218,11 +225,9 @@ def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): target_pose.pose.orientation.z = quat[2] target_pose.pose.orientation.w = quat[3] - # 目標位置・姿勢の制約（Constraints）を設定 goal_constraints = Constraints() goal_constraints.name = 'tolerance_goal' - # 位置制約を設定 position_constraint = PositionConstraint() position_constraint.header.frame_id = 'crane_plus_base' position_constraint.link_name = 'crane_plus_link_tcp' @@ -235,7 +240,6 @@ def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): position_constraint.constraint_region = tolerance_region position_constraint.weight = 1.0 - # 姿勢制約を設定 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = 'crane_plus_base' orientation_constraint.link_name = 'crane_plus_link_tcp' @@ -251,7 +255,6 @@ def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): self.arm.set_start_state_to_current_state() self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) - # 計画および実行 return plan_and_execute( self.crane_plus, self.arm, From d9b8274457796c324c407e8244f7259015148351 Mon Sep 17 00:00:00 2001 From: motty Date: Tue, 30 Jun 2026 18:21:44 +0900 Subject: [PATCH 5/8] =?UTF-8?q?crane=5Fplus=20examples=20=E3=82=92crane=5F?= =?UTF-8?q?x7=E3=81=AB=E5=90=88=E3=82=8F=E3=81=9B=E3=81=A6=E3=83=AA?= =?UTF-8?q?=E3=83=95=E3=82=A1=E3=82=AF=E3=82=BF=E3=83=AA=E3=83=B3=E3=82=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - C++: Nodeを継承するクラスをノードを受け取るstandaloneクラスに統一（pick_and_place_tf除く） - C++: camelCaseメソッド名をsnake_caseに統一（move_arm_to_named_pose, move_gripper_angle等） - pick_and_place: Poseオブジェクトのメンバ変数をcontrol_arm(x,y,z,r,p,y)パターンに変更 - pick_and_place: 座標定数をAPPROACH/PICK/PLACE/LIFTING_HEIGHTとしてmainに宣言 - pick_and_place_tf: privateアンダースコア付き定数をpublic static定数に変更 - pick_and_place_tf: set_constraints/clear_constraintsメソッドを追加 - Python: クラスレベル定数（GRIPPER_*）を追加し、selfからクラス参照に統一 - Python: pick_and_place_tfの座標定数をインスタンス変数からクラス変数に変更 Co-Authored-By: Claude Sonnet 4.6 --- crane_plus_examples/src/gripper_control.cpp | 49 ++-- crane_plus_examples/src/joint_values.cpp | 58 ++-- crane_plus_examples/src/pick_and_place.cpp | 178 ++++++----- crane_plus_examples/src/pick_and_place_tf.cpp | 277 ++++++++++-------- crane_plus_examples/src/pose_groupstate.cpp | 39 +-- .../crane_plus_examples_py/gripper_control.py | 20 +- .../crane_plus_examples_py/joint_values.py | 16 +- .../crane_plus_examples_py/pick_and_place.py | 141 +++++---- .../pick_and_place_tf.py | 218 +++++++------- 9 files changed, 512 insertions(+), 484 deletions(-) diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp index c5ae188b..5216326a 100644 --- a/crane_plus_examples/src/gripper_control.cpp +++ b/crane_plus_examples/src/gripper_control.cpp @@ -16,32 +16,32 @@ // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes // /run_move_group/src/run_move_group.cpp -#include #include +#include "angles/angles.h" #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -class GripperController : public rclcpp::Node +class GripperControl { public: - explicit GripperController(const rclcpp::NodeOptions & node_options) - : Node("gripper_control", node_options) - { - } + // グリッパの開閉角度 + inline static const double GRIPPER_DEFAULT = 0.0; + inline static const double GRIPPER_OPEN = angles::from_degrees(-30.0); + inline static const double GRIPPER_CLOSE = angles::from_degrees(30.0); - // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す - void initializeMoveGroup() + // ノードを受け取り、グリッパのMoveGroupInterfaceを初期化する + explicit GripperControl(rclcpp::Node::SharedPtr node) { - move_group_gripper_ = std::make_shared(shared_from_this(), "gripper"); - move_group_gripper_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper_ = std::make_shared(node, "gripper"); + move_group_gripper_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 } - // グリッパの開閉角度を設定して動かす - void moveGripperAngle(const double angle) + // グリッパを角度[rad]を指定して開閉する + void move_gripper_angle(const double angle) { auto joint_values = move_group_gripper_->getCurrentJointValues(); joint_values[0] = angle; @@ -53,40 +53,29 @@ class GripperController : public rclcpp::Node std::shared_ptr move_group_gripper_; }; -double toRadians(const double deg_angle) -{ - return deg_angle * M_PI / 180.0; -} - int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - - auto gripper_controller = std::make_shared(node_options); + auto node = rclcpp::Node::make_shared("gripper_control", node_options); // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる - std::thread spin_thread([gripper_controller]() { - rclcpp::spin(gripper_controller); - }); + std::thread spin_thread([node]() {rclcpp::spin(node);}); - gripper_controller->initializeMoveGroup(); + GripperControl controller(node); // グリッパを閉じる - gripper_controller->moveGripperAngle(toRadians(30.0)); + controller.move_gripper_angle(GripperControl::GRIPPER_CLOSE); // グリッパを開く - gripper_controller->moveGripperAngle(toRadians(-30.0)); + controller.move_gripper_angle(GripperControl::GRIPPER_OPEN); // グリッパを0度にする - gripper_controller->moveGripperAngle(toRadians(0.0)); + controller.move_gripper_angle(GripperControl::GRIPPER_DEFAULT); // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); - if (spin_thread.joinable()) { - spin_thread.join(); - } - + spin_thread.join(); return 0; } diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp index eb5609fa..61fff470 100644 --- a/crane_plus_examples/src/joint_values.cpp +++ b/crane_plus_examples/src/joint_values.cpp @@ -16,47 +16,42 @@ // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes // /run_move_group/src/run_move_group.cpp -#include #include #include +#include "angles/angles.h" #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -class JointValuesController : public rclcpp::Node +class JointValues { public: - explicit JointValuesController(const rclcpp::NodeOptions & node_options) - : Node("joint_values", node_options) + // ノードを受け取り、アームのMoveGroupInterfaceを初期化する + explicit JointValues(rclcpp::Node::SharedPtr node) { - } - - // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す - void initializeMoveGroup() - { - move_group_arm_ = std::make_shared(shared_from_this(), "arm_tcp"); - move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm_ = std::make_shared(node, "arm_tcp"); + move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 } - // SRDFに定義されている名前付きの姿勢に移動する - void moveArmToNamedPose(const std::string & name) + // SRDFに定義された姿勢名でアームを動かす + void move_arm_to_named_pose(const std::string & name) { move_group_arm_->setNamedTarget(name); move_group_arm_->move(); } - // 指定した関節角度にアームを動かす - void moveArmToJointValues(const std::vector & joint_values) + // 各ジョイント角度[rad]を指定してアームを動かす + void move_arm_joint_values(const std::vector & joint_values) { move_group_arm_->setJointValueTarget(joint_values); move_group_arm_->move(); } - // 現在の関節角度を取得する - std::vector getCurrentJointValues() + // アームの現在のジョイント角度を取得する + std::vector get_current_arm_joint_values() { return move_group_arm_->getCurrentJointValues(); } @@ -65,45 +60,34 @@ class JointValuesController : public rclcpp::Node std::shared_ptr move_group_arm_; }; -double toRadians(const double deg_angle) -{ - return deg_angle * M_PI / 180.0; -} - int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - - auto arm_controller = std::make_shared(node_options); + auto node = rclcpp::Node::make_shared("joint_values", node_options); // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる - std::thread spin_thread([arm_controller]() { - rclcpp::spin(arm_controller); - }); + std::thread spin_thread([node]() {rclcpp::spin(node);}); - arm_controller->initializeMoveGroup(); + JointValues controller(node); // verticalの姿勢から開始 - arm_controller->moveArmToNamedPose("vertical"); + controller.move_arm_to_named_pose("vertical"); // 各関節を順番に45度に動かす - auto joint_values = arm_controller->getCurrentJointValues(); - const double TARGET_ANGLE = toRadians(45.0); + const double TARGET_ANGLE = angles::from_degrees(45.0); + auto joint_values = controller.get_current_arm_joint_values(); for (size_t i = 0; i < joint_values.size(); i++) { joint_values[i] = TARGET_ANGLE; - arm_controller->moveArmToJointValues(joint_values); + controller.move_arm_joint_values(joint_values); } // verticalの姿勢に戻る - arm_controller->moveArmToNamedPose("vertical"); + controller.move_arm_to_named_pose("vertical"); // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); - if (spin_thread.joinable()) { - spin_thread.join(); - } - + spin_thread.join(); return 0; } diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp index 1d7b8c42..f9de2b64 100644 --- a/crane_plus_examples/src/pick_and_place.cpp +++ b/crane_plus_examples/src/pick_and_place.cpp @@ -16,108 +16,78 @@ // https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes // /run_move_group/src/run_move_group.cpp -#include #include +#include "angles/angles.h" #include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/quaternion.hpp" #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -class PickAndPlaceController : public rclcpp::Node +class PickAndPlace { public: - explicit PickAndPlaceController(const rclcpp::NodeOptions & node_options) - : Node("pick_and_place", node_options) - { - // グリッパの開閉角 - GRIPPER_DEFAULT = 0.0; - GRIPPER_OPEN = toRadians(-30.0); - GRIPPER_CLOSE = toRadians(10.0); - - // 物体の頭上の位置 - ABOVE_POSE = makePose(0.0, -0.21, 0.17, 0.0, 90.0, -90.0); - // 物体を掴む直前・直後の位置（持ち上げた高さ） - PRE_AND_POST_GRASP_POSE = makePose(0.0, -0.1, 0.05, 0.0, 180.0, -90.0); - // 物体を掴む位置 - GRASP_POSE = makePose(0.0, -0.1, 0.02, 0.0, 180.0, -90.0); - // 物体を置く位置 - RELEASE_POSE = makePose(0.25, 0.0, 0.06, 0.0, 90.0, 0.0); - } + // グリッパの開閉角度 + inline static const double GRIPPER_DEFAULT = 0.0; + inline static const double GRIPPER_OPEN = angles::from_degrees(-30.0); + inline static const double GRIPPER_CLOSE = angles::from_degrees(10.0); - // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す - void initializeMoveGroup() + // ノードを受け取り、アーム・グリッパのMoveGroupInterfaceを初期化する + explicit PickAndPlace(rclcpp::Node::SharedPtr node) { - move_group_arm_ = std::make_shared(shared_from_this(), "arm_tcp"); - move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm_ = std::make_shared(node, "arm_tcp"); + move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 // IKの成功率を向上させるため、目標位置姿勢の許容範囲を小さく設定 move_group_arm_->setGoalPositionTolerance(1e-5); move_group_arm_->setGoalOrientationTolerance(1e-4); - move_group_gripper_ = std::make_shared(shared_from_this(), "gripper"); - move_group_gripper_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper_ = std::make_shared(node, "gripper"); + move_group_gripper_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 } - // SRDFに定義されている名前付きの姿勢に移動する - void moveArmToNamedPose(const std::string & name) + // SRDFに定義された姿勢名でアームを動かす + void move_arm_to_named_pose(const std::string & name) { move_group_arm_->setNamedTarget(name); move_group_arm_->move(); } - // 指定した位置姿勢にアームを動かす - void moveArmToPose(const geometry_msgs::msg::Pose & pose) + // アームを目標位置・姿勢（Pose）に動かす + void move_arm_to_pose(const geometry_msgs::msg::Pose & pose) { move_group_arm_->setPoseTarget(pose); move_group_arm_->move(); } - // グリッパの開閉角度を設定して動かす - void moveGripperAngle(const double angle) - { - auto joint_values = move_group_gripper_->getCurrentJointValues(); - joint_values[0] = angle; - move_group_gripper_->setJointValueTarget(joint_values); - move_group_gripper_->move(); - } - - // 目標位置姿勢の定数 - geometry_msgs::msg::Pose ABOVE_POSE; - geometry_msgs::msg::Pose PRE_AND_POST_GRASP_POSE; - geometry_msgs::msg::Pose GRASP_POSE; - geometry_msgs::msg::Pose RELEASE_POSE; - - // グリッパ角度の定数 - double GRIPPER_DEFAULT; - double GRIPPER_OPEN; - double GRIPPER_CLOSE; - -private: - // x, y, z[m]とroll, pitch, yaw[deg]からPoseを生成する - static geometry_msgs::msg::Pose makePose( + // アームを目標位置（x, y, z [m]）・姿勢（roll, pitch, yaw [deg]）に動かす + void control_arm( const double x, const double y, const double z, const double roll, const double pitch, const double yaw) { - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = z; + geometry_msgs::msg::Pose target_pose; tf2::Quaternion q; - q.setRPY(toRadians(roll), toRadians(pitch), toRadians(yaw)); - pose.orientation = tf2::toMsg(q); - return pose; + target_pose.position.x = x; + target_pose.position.y = y; + target_pose.position.z = z; + q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw)); + target_pose.orientation = tf2::toMsg(q); + move_arm_to_pose(target_pose); } - static double toRadians(const double deg_angle) + // グリッパを角度[rad]を指定して開閉する + void move_gripper_angle(const double angle) { - return deg_angle * M_PI / 180.0; + auto joint_values = move_group_gripper_->getCurrentJointValues(); + joint_values[0] = angle; + move_group_gripper_->setJointValueTarget(joint_values); + move_group_gripper_->move(); } +private: std::shared_ptr move_group_arm_; std::shared_ptr move_group_gripper_; }; @@ -127,48 +97,76 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - - auto controller = std::make_shared(node_options); + auto node = rclcpp::Node::make_shared("pick_and_place", node_options); // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる - std::thread spin_thread([controller]() { - rclcpp::spin(controller); - }); - - controller->initializeMoveGroup(); + std::thread spin_thread([node]() {rclcpp::spin(node);}); + + PickAndPlace controller(node); + + // 物体上方のアプローチ位置のXYZ[m]とRPY[deg] + const double APPROACH_X = 0.0; + const double APPROACH_Y = -0.21; + const double APPROACH_Z = 0.17; + const double APPROACH_ROLL = 0.0; + const double APPROACH_PITCH = 90.0; + const double APPROACH_YAW = -90.0; + + // アプローチ・退避時の高さ + const double LIFTING_HEIGHT = 0.05; + + // 掴む位置（ピック位置）のXYZ[m]とRPY[deg] + const double PICK_X = 0.0; + const double PICK_Y = -0.1; + const double PICK_Z = 0.02; + const double PICK_ROLL = 0.0; + const double PICK_PITCH = 180.0; + const double PICK_YAW = -90.0; + + // 置く位置（プレース位置）のXYZ[m]とRPY[deg] + const double PLACE_X = 0.25; + const double PLACE_Y = 0.0; + const double PLACE_Z = 0.06; + const double PLACE_ROLL = 0.0; + const double PLACE_PITCH = 90.0; + const double PLACE_YAW = 0.0; // 初期姿勢 - controller->moveArmToNamedPose("vertical"); - controller->moveGripperAngle(controller->GRIPPER_DEFAULT); + controller.move_arm_to_named_pose("vertical"); + controller.move_gripper_angle(PickAndPlace::GRIPPER_DEFAULT); // ピック準備 - controller->moveArmToNamedPose("home"); - controller->moveGripperAngle(controller->GRIPPER_OPEN); - controller->moveArmToPose(controller->ABOVE_POSE); - controller->moveArmToPose(controller->PRE_AND_POST_GRASP_POSE); + controller.move_arm_to_named_pose("home"); + controller.move_gripper_angle(PickAndPlace::GRIPPER_OPEN); + // 物体上方へ移動 + controller.control_arm(APPROACH_X, APPROACH_Y, APPROACH_Z, APPROACH_ROLL, APPROACH_PITCH, + APPROACH_YAW); + // 物体直上まで降りる + controller.control_arm(PICK_X, PICK_Y, LIFTING_HEIGHT, PICK_ROLL, PICK_PITCH, PICK_YAW); // ピック動作 - controller->moveArmToPose(controller->GRASP_POSE); - controller->moveGripperAngle(controller->GRIPPER_CLOSE); - controller->moveArmToPose(controller->PRE_AND_POST_GRASP_POSE); - - // プレース準備 - controller->moveArmToNamedPose("home"); + // 掴む位置まで降りる + controller.control_arm(PICK_X, PICK_Y, PICK_Z, PICK_ROLL, PICK_PITCH, PICK_YAW); + // 掴む + controller.move_gripper_angle(PickAndPlace::GRIPPER_CLOSE); + // 持ち上げる + controller.control_arm(PICK_X, PICK_Y, LIFTING_HEIGHT, PICK_ROLL, PICK_PITCH, PICK_YAW); // プレース動作 - controller->moveArmToPose(controller->RELEASE_POSE); - controller->moveGripperAngle(controller->GRIPPER_OPEN); + // homeを経由してプレース位置へ移動 + controller.move_arm_to_named_pose("home"); + // 置く位置まで降ろす + controller.control_arm(PLACE_X, PLACE_Y, PLACE_Z, PLACE_ROLL, PLACE_PITCH, PLACE_YAW); + // 離す + controller.move_gripper_angle(PickAndPlace::GRIPPER_OPEN); // 終了動作 - controller->moveArmToNamedPose("home"); - controller->moveArmToNamedPose("vertical"); - controller->moveGripperAngle(controller->GRIPPER_DEFAULT); + controller.move_arm_to_named_pose("home"); + controller.move_arm_to_named_pose("vertical"); + controller.move_gripper_angle(PickAndPlace::GRIPPER_DEFAULT); // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); - if (spin_thread.joinable()) { - spin_thread.join(); - } - + spin_thread.join(); return 0; } diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp index a6e2363b..fcff462c 100644 --- a/crane_plus_examples/src/pick_and_place_tf.cpp +++ b/crane_plus_examples/src/pick_and_place_tf.cpp @@ -24,17 +24,14 @@ #include "angles/angles.h" #include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/convert.hpp" #include "tf2/exceptions.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" -#include "std_msgs/msg/string.hpp" +#include "tf2_ros/transform_listener.h" using namespace std::chrono_literals; using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; @@ -42,12 +39,37 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; class PickAndPlaceTf : public rclcpp::Node { public: + // 位置姿勢パラメータ（x, y, z [m], roll, pitch, yaw [deg]） + struct PoseParams + { + double x, y, z, roll, pitch, yaw; + }; + + // グリッパの開閉角度 + inline static const double GRIPPER_DEFAULT = 0.0; + inline static const double GRIPPER_OPEN = angles::from_degrees(-30.0); + inline static const double GRIPPER_CLOSE = angles::from_degrees(10.0); + + // 待機姿勢の位置姿勢（x, y, z [m], roll, pitch, yaw [deg]） + inline static const PoseParams STANDBY = {0.0, 0.0, 0.3, 0.0, 0.0, 0.0}; + + // 把持アプローチ時の高さおよびピッチ角 + inline static const double GRASP_HEIGHT = 0.04; + inline static const double GRASP_PITCH = 90.0; + + // 搬送時の中間姿勢 + inline static const PoseParams TRANSIT_1 = {0.12, 0.0, 0.17, 0.0, 90.0, 0.0}; + inline static const PoseParams TRANSIT_2 = {0.0, -0.12, 0.17, 0.0, 90.0, -90.0}; + + // 置く位置（プレース位置）とその退避姿勢 + inline static const PoseParams PLACE = {0.0, -0.25, 0.05, 0.0, 90.0, -90.0}; + inline static const PoseParams PLACE_RETRACT = {0.0, -0.25, 0.10, 0.0, 90.0, -90.0}; + PickAndPlaceTf( rclcpp::Node::SharedPtr move_group_arm_node, rclcpp::Node::SharedPtr move_group_gripper_node) : Node("pick_and_place_tf_node") { - // アームおよびグリッパ制御用のMoveGroupInterfaceの初期化とパラメータ設定 move_group_arm_ = std::make_shared(move_group_arm_node, "arm_tcp"); move_group_arm_->setMaxVelocityScalingFactor(1.0); move_group_arm_->setMaxAccelerationScalingFactor(1.0); @@ -60,11 +82,69 @@ class PickAndPlaceTf : public rclcpp::Node move_group_arm_->setGoalPositionTolerance(1e-5); move_group_arm_->setGoalOrientationTolerance(1e-4); - // 初期姿勢としてSRDFに定義されている 'home' の位置姿勢に移動 - move_group_arm_->setNamedTarget("home"); + // SRDFに定義されている "home" の姿勢に移動 + move_arm_to_named_pose("home"); + + // アームの可動範囲制限を設定してから待機姿勢に移動する + set_constraints(); + control_arm(STANDBY); + + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // 0.5秒ごとにon_timerを呼び出すタイマーを作成 + timer_ = this->create_wall_timer(500ms, std::bind(&PickAndPlaceTf::on_timer, this)); + } + + // グリッパを角度[rad]を指定して開閉する + void move_gripper_angle(const double angle) + { + auto joint_values = move_group_gripper_->getCurrentJointValues(); + joint_values[0] = angle; + move_group_gripper_->setJointValueTarget(joint_values); + move_group_gripper_->move(); + } + + // アームを目標位置・姿勢（Pose）に動かす + void move_arm_to_pose(const geometry_msgs::msg::Pose & pose) + { + move_group_arm_->setPoseTarget(pose); + move_group_arm_->move(); + } + + // アームを目標位置（x, y, z [m]）・姿勢（roll, pitch, yaw [deg]）に動かす + bool control_arm( + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw) + { + geometry_msgs::msg::Pose target_pose; + tf2::Quaternion q; + target_pose.position.x = x; + target_pose.position.y = y; + target_pose.position.z = z; + q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw)); + target_pose.orientation = tf2::toMsg(q); + move_group_arm_->setPoseTarget(target_pose); + moveit::core::MoveItErrorCode result = move_group_arm_->move(); + return result.val == moveit::core::MoveItErrorCode::SUCCESS; + } + + // 位置姿勢パラメータ（PoseParams）を使ってアームを動かす + bool control_arm(const PoseParams & pose) + { + return control_arm(pose.x, pose.y, pose.z, pose.roll, pose.pitch, pose.yaw); + } + + // SRDFに定義された姿勢名でアームを動かす + void move_arm_to_named_pose(const std::string & name) + { + move_group_arm_->setNamedTarget(name); move_group_arm_->move(); + } - // アームの可動範囲制限（Path Constraints）を設定して、関節の動きを制限する + // アームの関節の一部に可動制限を設定する + void set_constraints() + { moveit_msgs::msg::Constraints constraints; constraints.name = "arm_constraints"; @@ -84,32 +164,15 @@ class PickAndPlaceTf : public rclcpp::Node constraints.joint_constraints.push_back(joint_constraint); move_group_arm_->setPathConstraints(constraints); + } - // 初期位置としての待機姿勢に移動 - moveArmToPose(STANDBY_POSITION_); - - // TFの受信に必要なBufferとListenerを初期化 - tf_buffer_ = - std::make_unique(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); - - // 定期的にTFを監視してピッキングをトリガーするためのタイマーを設定（0.5秒周期） - timer_ = this->create_wall_timer( - 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); + // 設定された関節可動制限をクリアする + void clear_constraints() + { + move_group_arm_->clearPathConstraints(); } private: - // 位置姿勢のパラメータを格納する構造体 - struct PoseParams { - double x; - double y; - double z; - double roll; - double pitch; - double yaw; - }; - void on_timer() { // target_0（把持対象）のTFを取得 @@ -127,96 +190,74 @@ class PickAndPlaceTf : public rclcpp::Node } rclcpp::Time now = this->get_clock()->now(); - const std::chrono::nanoseconds FILTERING_TIME = 2s; - const std::chrono::nanoseconds STOP_TIME_THRESHOLD = 3s; - const float DISTANCE_THRESHOLD = 0.01; - tf2::Stamped tf; - tf2::convert(tf_msg, tf); - const auto TF_ELAPSED_TIME = now.nanoseconds() - tf.stamp_.time_since_epoch().count(); - const auto TF_STOP_TIME = now.nanoseconds() - tf_past_.stamp_.time_since_epoch().count(); - - // 一定時間（2秒）以内に更新された有効なTFのみ処理する - if (TF_ELAPSED_TIME < FILTERING_TIME.count()) { - double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length(); - - // 前回位置からの移動量が閾値未満（静止状態）であるかを判定 - if (tf_diff < DISTANCE_THRESHOLD) { - // 一定時間（3秒）以上静止し続けている場合、ピッキング動作を開始 - if (TF_STOP_TIME > STOP_TIME_THRESHOLD.count()) { - picking(tf.getOrigin()); - } - } else { - // 移動した場合は過去のTF位置情報を更新 - tf_past_ = tf; - } - } - } + const auto FILTERING_TIME = rclcpp::Duration(2s); + const auto STOP_TIME_THRESHOLD = rclcpp::Duration(3s); + const double DISTANCE_THRESHOLD = 0.01; + const double TARGET_Z_MIN_LIMIT = 0.04; - void picking(tf2::Vector3 target_position) - { - // 1. 掴み動作の準備とターゲットへの正対 - moveGripperAngle(GRIPPER_OPEN_); - - double x = target_position.x(); - double y = target_position.y(); - double theta_rad = std::atan2(y, x); - double theta_deg = theta_rad * 180.0 / 3.1415926535; + tf2::Stamped tf_current; + tf2::convert(tf_msg, tf_current); - moveArmToPose(0.0, 0.0, STANDBY_POSITION_.z, 0, 0, theta_deg); + const auto tf_elapsed_time = now - rclcpp::Time(tf_msg.header.stamp, RCL_ROS_TIME); + const auto tf_stop_time = + now - rclcpp::Time(tf_past_.stamp_.time_since_epoch().count(), RCL_ROS_TIME); - // 2. ターゲット位置へのアプローチと掴み動作 - if (!moveArmToPose(x, y, GRASP_HEIGHT_, 0, GRASP_PITCH_, theta_deg)) { - // アーム動作に失敗した場合は初期姿勢に戻る - moveArmToPose(STANDBY_POSITION_); + // 現在時刻から2秒以内に受け取ったtfを使用 + if (tf_elapsed_time > FILTERING_TIME) { return; } - moveGripperAngle(GRIPPER_CLOSE_); + double tf_diff = (tf_past_.getOrigin() - tf_current.getOrigin()).length(); - // 3. 搬送および配置動作 - moveArmToPose(TRANSIT_POSE_1_); - moveArmToPose(TRANSIT_POSE_2_); - moveArmToPose(PLACE_POSE_); + // 把持対象の位置が停止していることを判定 + if (tf_diff > DISTANCE_THRESHOLD) { + tf_past_ = tf_current; + return; + } - moveGripperAngle(GRIPPER_OPEN_); + // 把持対象が3秒以上停止している場合ピッキング動作開始 + if (tf_stop_time < STOP_TIME_THRESHOLD) { + return; + } - // 4. 待機姿勢への復帰 - moveArmToPose(PLACE_RETRACT_POSE_); - moveArmToPose(STANDBY_POSITION_); - moveGripperAngle(GRIPPER_DEFAULT_); - } + // 把持対象が低すぎる場合は把持位置を調整 + if (tf_current.getOrigin().z() < TARGET_Z_MIN_LIMIT) { + tf_current.getOrigin().setZ(TARGET_Z_MIN_LIMIT); + } - // グリッパの開閉角度を設定して動かす - void moveGripperAngle(const double angle) - { - auto joint_values = move_group_gripper_->getCurrentJointValues(); - joint_values[0] = angle; - move_group_gripper_->setJointValueTarget(joint_values); - move_group_gripper_->move(); + picking(tf_current.getOrigin()); } - // 構造体で指定した位置姿勢にアームを動かす - bool moveArmToPose(const PoseParams & pose) + void picking(tf2::Vector3 target_position) { - return moveArmToPose(pose.x, pose.y, pose.z, pose.roll, pose.pitch, pose.yaw); - } + // 何かを掴んでいた時のためにハンドを開閉 + move_gripper_angle(GRIPPER_OPEN); - // x, y, z[m]とroll, pitch, yaw[deg]で指定した位置姿勢にアームを動かす - bool moveArmToPose( - const double x, const double y, const double z, - const double roll, const double pitch, const double yaw) - { - geometry_msgs::msg::Pose target_pose; - tf2::Quaternion q; - target_pose.position.x = x; - target_pose.position.y = y; - target_pose.position.z = z; - q.setRPY(angles::from_degrees(roll), angles::from_degrees(pitch), angles::from_degrees(yaw)); - target_pose.orientation = tf2::toMsg(q); - move_group_arm_->setPoseTarget(target_pose); - // アーム動作の成否を取得 - moveit::core::MoveItErrorCode result = move_group_arm_->move(); - return result.val == moveit::core::MoveItErrorCode::SUCCESS; + double x = target_position.x(); + double y = target_position.y(); + double theta_deg = std::atan2(y, x) * 180.0 / M_PI; + + // ターゲットの正面に向ける + control_arm(0.0, 0.0, STANDBY.z, 0, 0, theta_deg); + + // ピック動作（掴みに行く） + if (!control_arm(x, y, GRASP_HEIGHT, 0, GRASP_PITCH, theta_deg)) { + // アーム動作に失敗した場合は待機姿勢に戻る + control_arm(STANDBY); + return; + } + move_gripper_angle(GRIPPER_CLOSE); + + // プレース動作（移動して置く） + control_arm(TRANSIT_1); + control_arm(TRANSIT_2); + control_arm(PLACE); + move_gripper_angle(GRIPPER_OPEN); + + // 待機姿勢に戻る + control_arm(PLACE_RETRACT); + control_arm(STANDBY); + move_gripper_angle(GRIPPER_DEFAULT); } std::shared_ptr move_group_arm_; @@ -225,24 +266,6 @@ class PickAndPlaceTf : public rclcpp::Node std::shared_ptr tf_listener_{nullptr}; rclcpp::TimerBase::SharedPtr timer_{nullptr}; tf2::Stamped tf_past_; - - // グリッパ角度の定数 - const double GRIPPER_DEFAULT_ = 0.0; - const double GRIPPER_OPEN_ = angles::from_degrees(-30.0); - const double GRIPPER_CLOSE_ = angles::from_degrees(10.0); - - // 待機姿勢の位置姿勢（x, y, z [m], roll, pitch, yaw [deg]） - const PoseParams STANDBY_POSITION_ = {0.0, 0.0, 0.3, 0.0, 0.0, 0.0}; - - // 把持アプローチ時の高さおよびピッチ角 - const double GRASP_HEIGHT_ = 0.04; - const double GRASP_PITCH_ = 90.0; - - // 搬送時の中間姿勢およびプレース位置姿勢 - const PoseParams TRANSIT_POSE_1_ = {0.12, 0.0, 0.17, 0.0, 90.0, 0.0}; - const PoseParams TRANSIT_POSE_2_ = {0.0, -0.12, 0.17, 0.0, 90.0, -90.0}; - const PoseParams PLACE_POSE_ = {0.0, -0.25, 0.05, 0.0, 90.0, -90.0}; - const PoseParams PLACE_RETRACT_POSE_ = {0.0, -0.25, 0.10, 0.0, 90.0, -90.0}; }; int main(int argc, char ** argv) @@ -253,14 +276,18 @@ int main(int argc, char ** argv) auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options); auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); + // タイマーとTFリスナーを持つため、MultiThreadedExecutorを使用する rclcpp::executors::MultiThreadedExecutor exec; auto pick_and_place_tf_node = std::make_shared( move_group_arm_node, move_group_gripper_node); + exec.add_node(pick_and_place_tf_node); exec.add_node(move_group_arm_node); exec.add_node(move_group_gripper_node); exec.spin(); + + pick_and_place_tf_node->clear_constraints(); rclcpp::shutdown(); return 0; } diff --git a/crane_plus_examples/src/pose_groupstate.cpp b/crane_plus_examples/src/pose_groupstate.cpp index c72fcbee..0e4bb753 100644 --- a/crane_plus_examples/src/pose_groupstate.cpp +++ b/crane_plus_examples/src/pose_groupstate.cpp @@ -23,24 +23,19 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; -class PoseGroupstateController : public rclcpp::Node +class PoseGroupstate { public: - explicit PoseGroupstateController(const rclcpp::NodeOptions & node_options) - : Node("pose_groupstate", node_options) + // ノードを受け取り、アームのMoveGroupInterfaceを初期化する + explicit PoseGroupstate(rclcpp::Node::SharedPtr node) { - } - - // MoveGroupInterfaceはshared_from_this()を使うため、コンストラクタ後に呼び出す - void initializeMoveGroup() - { - move_group_arm_ = std::make_shared(shared_from_this(), "arm_tcp"); - move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm_ = std::make_shared(node, "arm_tcp"); + move_group_arm_->setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm_->setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 } - // SRDFに定義されている名前付きの姿勢に移動する - void moveArmToNamedPose(const std::string & name) + // SRDFに定義された姿勢名でアームを動かす + void move_arm_to_named_pose(const std::string & name) { move_group_arm_->setNamedTarget(name); move_group_arm_->move(); @@ -55,26 +50,20 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - - auto arm_controller = std::make_shared(node_options); + auto node = rclcpp::Node::make_shared("pose_groupstate", node_options); // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる - std::thread spin_thread([arm_controller]() { - rclcpp::spin(arm_controller); - }); + std::thread spin_thread([node]() {rclcpp::spin(node);}); - arm_controller->initializeMoveGroup(); + PoseGroupstate controller(node); // SRDFに定義されている名前付き姿勢を順番に実行する - arm_controller->moveArmToNamedPose("home"); - arm_controller->moveArmToNamedPose("vertical"); - arm_controller->moveArmToNamedPose("home"); + controller.move_arm_to_named_pose("home"); + controller.move_arm_to_named_pose("vertical"); + controller.move_arm_to_named_pose("home"); // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する rclcpp::shutdown(); - if (spin_thread.joinable()) { - spin_thread.join(); - } - + spin_thread.join(); return 0; } diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 7b82bc18..9c2ef0d4 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -26,21 +26,27 @@ from rclpy.logging import get_logger -class GripperController: +class GripperControl: + # グリッパの開閉角度 + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(30.0) + def __init__(self): # MoveItPyのインスタンスを生成し、planning componentを取得 self.crane_plus = MoveItPy(node_name='gripper_control') self.gripper = self.crane_plus.get_planning_component('gripper') self.robot_model = self.crane_plus.get_robot_model() - # プランニングパラメータの設定 + # プランニングの設定（動作プランナーと速度・加速度スケール） self.plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.logger = get_logger('gripper_control') def move_gripper_angle(self, angle): + # グリッパを角度[rad]を指定して開閉する self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) @@ -56,16 +62,16 @@ def move_gripper_angle(self, angle): def main(args=None): rclpy.init(args=args) - controller = GripperController() + controller = GripperControl() # グリッパを閉じる - controller.move_gripper_angle(math.radians(30.0)) + controller.move_gripper_angle(GripperControl.GRIPPER_CLOSE) # グリッパを開く - controller.move_gripper_angle(math.radians(-30.0)) + controller.move_gripper_angle(GripperControl.GRIPPER_OPEN) # グリッパを0度にする - controller.move_gripper_angle(math.radians(0.0)) + controller.move_gripper_angle(GripperControl.GRIPPER_DEFAULT) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 57d87b3d..4b8fed86 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -27,7 +27,7 @@ from rclpy.logging import get_logger -class JointValuesController: +class JointValues: def __init__(self): # MoveItPyのインスタンスを生成し、planning componentを取得 self.crane_plus = MoveItPy(node_name='joint_values') @@ -35,9 +35,9 @@ def __init__(self): self.robot_model = self.crane_plus.get_robot_model() self.robot_state = RobotState(self.robot_model) - # プランニングパラメータの設定 + # プランニングの設定（動作プランナーと速度・加速度スケール） self.plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.logger = get_logger('joint_values') @@ -51,7 +51,7 @@ def move_arm_to_named_pose(self, name): single_plan_parameters=self.plan_request_params, ) - def move_arm_to_joint_values(self, joint_values_dict): + def move_arm_joint_values(self, joint_values_dict): # 関節角度の辞書を指定してアームを動かす self.robot_state.joint_positions = joint_values_dict joint_constraint = construct_joint_constraint( @@ -65,7 +65,7 @@ def move_arm_to_joint_values(self, joint_values_dict): single_plan_parameters=self.plan_request_params, ) - def get_current_joint_values(self, joint_names): + def get_current_arm_joint_values(self, joint_names): # 現在の関節角度を名前付き辞書で取得する self.arm.set_start_state_to_current_state() current_state = self.arm.get_start_state() @@ -76,7 +76,7 @@ def get_current_joint_values(self, joint_names): def main(args=None): rclpy.init(args=args) - controller = JointValuesController() + controller = JointValues() joint_names = [ 'crane_plus_joint1', @@ -90,10 +90,10 @@ def main(args=None): controller.move_arm_to_named_pose('vertical') # 各関節を順番に45度に動かす - joint_values_dict = controller.get_current_joint_values(joint_names) + joint_values_dict = controller.get_current_arm_joint_values(joint_names) for joint_name in joint_names: joint_values_dict[joint_name] = target_joint_value - controller.move_arm_to_joint_values(joint_values_dict) + controller.move_arm_joint_values(joint_values_dict) # verticalの姿勢に戻る controller.move_arm_to_named_pose('vertical') diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 96baa212..433932c8 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -12,12 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -import copy import math from crane_plus_examples_py.utils import plan_and_execute -from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from geometry_msgs.msg import Pose, PoseStamped from moveit.core.robot_state import RobotState from moveit.planning import ( @@ -32,7 +31,12 @@ from shape_msgs.msg import SolidPrimitive -class PickAndPlaceController: +class PickAndPlace: + # グリッパの開閉角度 + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + def __init__(self): # MoveItPyのインスタンスを生成し、planning componentを取得 self.crane_plus = MoveItPy(node_name='pick_and_place') @@ -40,44 +44,29 @@ def __init__(self): self.gripper = self.crane_plus.get_planning_component('gripper') self.robot_model = self.crane_plus.get_robot_model() - # プランニングパラメータの設定 + # プランニングの設定（動作プランナーと速度・加速度スケール） self.arm_plan_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - self.arm_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.arm_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.arm_plan_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.gripper_plan_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - self.gripper_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.gripper_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.gripper_plan_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.logger = get_logger('pick_and_place') - # グリッパの開閉角 - self.GRIPPER_DEFAULT = 0.0 - self.GRIPPER_OPEN = math.radians(-30.0) - self.GRIPPER_CLOSE = math.radians(10.0) - - # 物体を持ち上げる高さ - LIFTING_HEIGHT = 0.03 - # 目標位置姿勢の定数 - self.ABOVE_POSE = self._make_pose(0.0, -0.21, 0.17, 0.0, 90.0, -90.0) - self.GRASP_POSE = self._make_pose(0.0, -0.1, 0.02, 0.0, 180.0, -90.0) - pre_post = copy.deepcopy(self.GRASP_POSE) - pre_post.position.z = LIFTING_HEIGHT - self.PRE_AND_POST_GRASP_POSE = pre_post - self.RELEASE_POSE = self._make_pose(0.25, 0.0, 0.06, 0.0, 90.0, 0.0) - - @staticmethod - def _make_pose(x, y, z, roll, pitch, yaw): - # x, y, z[m]とroll, pitch, yaw[deg]からPoseを生成する - quat = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True).as_quat() - return Pose( - position=Point(x=x, y=y, z=z), - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + def move_arm_to_named_pose(self, name): + # SRDFに定義されている名前付きの姿勢に移動する + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(configuration_name=name) + plan_and_execute( + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.arm_plan_params, ) - @staticmethod - def _make_goal_constraints(pose): - # 位置・姿勢の目標制約を生成する + def move_arm_to_pose(self, pose): + # 位置姿勢の制約を設定してアームを動かす + # （IKの精度を向上させるため、位置・姿勢の許容範囲を設定） POSITION_TOLERANCE = 0.00001 ORIENTATION_TOLERANCE = 0.0001 @@ -111,20 +100,7 @@ def _make_goal_constraints(pose): goal_constraints.position_constraints.append(position_constraint) goal_constraints.orientation_constraints.append(orientation_constraint) - return goal_constraints - def move_arm_to_named_pose(self, name): - # SRDFに定義されている名前付きの姿勢に移動する - self.arm.set_start_state_to_current_state() - self.arm.set_goal_state(configuration_name=name) - plan_and_execute( - self.crane_plus, self.arm, self.logger, - single_plan_parameters=self.arm_plan_params, - ) - - def move_arm_to_pose(self, pose): - # 位置姿勢の制約を設定してアームを動かす - goal_constraints = self._make_goal_constraints(pose) self.arm.set_start_state_to_current_state() self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( @@ -132,7 +108,21 @@ def move_arm_to_pose(self, pose): single_plan_parameters=self.arm_plan_params, ) + def control_arm(self, x, y, z, roll, pitch, yaw): + # アームを目標位置（x, y, z [m]）・姿勢（roll, pitch, yaw [deg]）に動かす + pose = Pose() + pose.position.x = x + pose.position.y = y + pose.position.z = z + quat = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True).as_quat() + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + self.move_arm_to_pose(pose) + def move_gripper_angle(self, angle): + # グリッパを角度[rad]を指定して開閉する self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) @@ -146,34 +136,69 @@ def move_gripper_angle(self, angle): def main(args=None): rclpy.init(args=args) - controller = PickAndPlaceController() + controller = PickAndPlace() + + # 物体上方のアプローチ位置のXYZ[m]とRPY[deg] + APPROACH_X = 0.0 + APPROACH_Y = -0.21 + APPROACH_Z = 0.17 + APPROACH_ROLL = 0.0 + APPROACH_PITCH = 90.0 + APPROACH_YAW = -90.0 + + # アプローチ・退避時の高さ + LIFTING_HEIGHT = 0.05 + + # 掴む位置（ピック位置）のXYZ[m]とRPY[deg] + PICK_X = 0.0 + PICK_Y = -0.1 + PICK_Z = 0.02 + PICK_ROLL = 0.0 + PICK_PITCH = 180.0 + PICK_YAW = -90.0 + + # 置く位置（プレース位置）のXYZ[m]とRPY[deg] + PLACE_X = 0.25 + PLACE_Y = 0.0 + PLACE_Z = 0.06 + PLACE_ROLL = 0.0 + PLACE_PITCH = 90.0 + PLACE_YAW = 0.0 # 初期姿勢 controller.move_arm_to_named_pose('vertical') - controller.move_gripper_angle(controller.GRIPPER_DEFAULT) + controller.move_gripper_angle(PickAndPlace.GRIPPER_DEFAULT) # ピック準備 controller.move_arm_to_named_pose('home') - controller.move_gripper_angle(controller.GRIPPER_OPEN) - controller.move_arm_to_pose(controller.ABOVE_POSE) - controller.move_arm_to_pose(controller.PRE_AND_POST_GRASP_POSE) + controller.move_gripper_angle(PickAndPlace.GRIPPER_OPEN) + # 物体上方へ移動 + controller.control_arm( + APPROACH_X, APPROACH_Y, APPROACH_Z, APPROACH_ROLL, APPROACH_PITCH, APPROACH_YAW + ) + # 物体直上まで降りる + controller.control_arm(PICK_X, PICK_Y, LIFTING_HEIGHT, PICK_ROLL, PICK_PITCH, PICK_YAW) # ピック動作 - controller.move_arm_to_pose(controller.GRASP_POSE) - controller.move_gripper_angle(controller.GRIPPER_CLOSE) - controller.move_arm_to_pose(controller.PRE_AND_POST_GRASP_POSE) - - # プレース準備 - controller.move_arm_to_named_pose('home') + # 掴む位置まで降りる + controller.control_arm(PICK_X, PICK_Y, PICK_Z, PICK_ROLL, PICK_PITCH, PICK_YAW) + # 掴む + controller.move_gripper_angle(PickAndPlace.GRIPPER_CLOSE) + # 持ち上げる + controller.control_arm(PICK_X, PICK_Y, LIFTING_HEIGHT, PICK_ROLL, PICK_PITCH, PICK_YAW) # プレース動作 - controller.move_arm_to_pose(controller.RELEASE_POSE) - controller.move_gripper_angle(controller.GRIPPER_OPEN) + # homeを経由してプレース位置へ移動 + controller.move_arm_to_named_pose('home') + # 置く位置まで降ろす + controller.control_arm(PLACE_X, PLACE_Y, PLACE_Z, PLACE_ROLL, PLACE_PITCH, PLACE_YAW) + # 離す + controller.move_gripper_angle(PickAndPlace.GRIPPER_OPEN) # 終了動作 controller.move_arm_to_named_pose('home') controller.move_arm_to_named_pose('vertical') - controller.move_gripper_angle(controller.GRIPPER_DEFAULT) + controller.move_gripper_angle(PickAndPlace.GRIPPER_DEFAULT) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index f0817848..ec879fd7 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -42,11 +42,31 @@ class PickAndPlaceTf(Node): + # グリッパの開閉角度 + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # 待機姿勢の位置姿勢（x, y, z [m], roll, pitch, yaw [deg]） + STANDBY = (0.0, 0.0, 0.3, 0.0, 0.0, 0.0) + + # 把持アプローチ時の高さおよびピッチ角 + GRASP_HEIGHT = 0.04 + GRASP_PITCH = 90.0 + + # 搬送時の中間姿勢 + TRANSIT_1 = (0.12, 0.0, 0.17, 0.0, 90.0, 0.0) + TRANSIT_2 = (0.0, -0.12, 0.17, 0.0, 90.0, -90.0) + + # 置く位置（プレース位置）とその退避姿勢 + PLACE = (0.0, -0.25, 0.05, 0.0, 90.0, -90.0) + PLACE_RETRACT = (0.0, -0.25, 0.10, 0.0, 90.0, -90.0) + def __init__(self): super().__init__('pick_and_place_tf') self.logger = self.get_logger() - # TFの受信に必要なBufferとListenerを初期化 + # TFリスナーの初期化 self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.tf_past = TransformStamped() @@ -55,78 +75,39 @@ def __init__(self): self.crane_plus = MoveItPy(node_name='moveit_py') self.logger.info('MoveItPy instance created') - # アームおよびグリッパ制御用のplanning componentを取得 + # アーム・グリッパ制御用 planning component self.arm = self.crane_plus.get_planning_component('arm_tcp') self.gripper = self.crane_plus.get_planning_component('gripper') - # ロボットモデルからRobotStateインスタンスを取得（グリッパの関節角設定等に使用） + # ロボットモデルの取得（ジョイント目標値の設定に使用） self.robot_model = self.crane_plus.get_robot_model() - # 計画時のパラメータ（PlanRequestParameters）を設定 - self.arm_plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - self.gripper_plan_request_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') - - # アームとグリッパの最大速度・加速度スケーリングファクタを設定 (0.0 ~ 1.0) - self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 - self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 - - self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 - self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 - - # グリッパの目標開閉角度を設定 - self.GRIPPER_DEFAULT = 0.0 - self.GRIPPER_OPEN = math.radians(-30.0) - self.GRIPPER_CLOSE = math.radians(10.0) + # プランニングの設定（動作プランナーと速度・加速度スケール） + self.arm_plan_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.arm_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.arm_plan_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # 待機姿勢の位置姿勢（x, y, z [m], roll, pitch, yaw [deg]） - self.STANDBY_POSITION = (0.0, 0.0, 0.3, 0.0, 0.0, 0.0) + self.gripper_plan_params = PlanRequestParameters(self.crane_plus, 'ompl_rrtc') + self.gripper_plan_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.gripper_plan_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # 搬送時の中間姿勢およびプレース位置姿勢 - self.TRANSIT_POSE_1 = (0.12, 0.0, 0.17, 0.0, 90.0, 0.0) - self.TRANSIT_POSE_2 = (0.0, -0.12, 0.17, 0.0, 90.0, -90.0) - self.PLACE_POSE = (0.0, -0.25, 0.05, 0.0, 90.0, -90.0) - self.PLACE_RETRACT_POSE = (0.0, -0.25, 0.10, 0.0, 90.0, -90.0) - - # 初期姿勢としてSRDFに定義されている 'home' の位置姿勢に移動 + # SRDFに定義されている "home" の姿勢に移動 self.arm.set_start_state_to_current_state() self.arm.set_goal_state(configuration_name='home') plan_and_execute( - self.crane_plus, - self.arm, - self.logger, - single_plan_parameters=self.arm_plan_request_params, + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.arm_plan_params, ) - # アームの可動範囲制限（Path Constraints）を設定して、関節の動きを制限する - constraints = Constraints() - constraints.name = 'arm_constraints' - - joint_constraint = JointConstraint() - joint_constraint.joint_name = 'crane_plus_joint1' - joint_constraint.position = 0.0 - joint_constraint.tolerance_above = math.radians(100) - joint_constraint.tolerance_below = math.radians(100) - joint_constraint.weight = 1.0 - constraints.joint_constraints.append(joint_constraint) - - joint_constraint = JointConstraint() - joint_constraint.joint_name = 'crane_plus_joint3' - joint_constraint.position = 0.0 - joint_constraint.tolerance_above = math.radians(0) - joint_constraint.tolerance_below = math.radians(180) - joint_constraint.weight = 1.0 - constraints.joint_constraints.append(joint_constraint) - - self.arm.set_path_constraints(constraints) - - # 初期位置としての待機姿勢に移動 - self.move_arm_to_pose(*self.STANDBY_POSITION) + # アームの可動範囲制限を設定してから待機姿勢に移動する + self.set_constraints() + self.control_arm(*self.STANDBY) - # 定期的にTFを監視してピッキングをトリガーするためのタイマーを設定（0.5秒周期） + # 0.5秒ごとにon_timerを呼び出すタイマーを作成 self.timer = self.create_timer(0.5, self.on_timer) def on_timer(self): - # target_0（把持対象）のTFを取得 + # target_0のtf位置姿勢を取得 try: tf_msg = self.tf_buffer.lookup_transform( 'crane_plus_base', 'target_0', rclpy.time.Time() @@ -140,76 +121,78 @@ def on_timer(self): STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3) DISTANCE_THRESHOLD = 0.01 - # ターゲットのTFが更新された時刻からの経過時間と、前回の位置から静止している時間を計算 tf_time = rclpy.time.Time.from_msg(tf_msg.header.stamp) - TF_ELAPSED_TIME = now_time - tf_time + tf_elapsed_time = now_time - tf_time tf_past_time = rclpy.time.Time.from_msg(self.tf_past.header.stamp) - TF_STOP_TIME = now_time - tf_past_time - - # 一定時間（2秒）以内に更新された有効なTFのみ処理する - if TF_ELAPSED_TIME < FILTERING_TIME: - tf_diff = np.linalg.norm( - [ - self.tf_past.transform.translation.x - tf_msg.transform.translation.x, - self.tf_past.transform.translation.y - tf_msg.transform.translation.y, - self.tf_past.transform.translation.z - tf_msg.transform.translation.z, - ] - ) + tf_stop_time = now_time - tf_past_time - # 前回位置からの移動量が閾値未満（静止状態）であるかを判定 - if tf_diff < DISTANCE_THRESHOLD: - # 一定時間（3秒）以上静止し続けている場合、ピッキング動作を開始 - if TF_STOP_TIME > STOP_TIME_THRESHOLD: - self.picking(tf_msg.transform.translation) - else: - # 移動した場合は過去のTF位置情報を更新 - self.tf_past = tf_msg + # 現在時刻から2秒以内に受け取ったtfを使用 + if tf_elapsed_time > FILTERING_TIME: + return + + tf_diff = np.linalg.norm( + [ + self.tf_past.transform.translation.x - tf_msg.transform.translation.x, + self.tf_past.transform.translation.y - tf_msg.transform.translation.y, + self.tf_past.transform.translation.z - tf_msg.transform.translation.z, + ] + ) + + # 把持対象の位置が停止していることを判定 + if tf_diff > DISTANCE_THRESHOLD: + self.tf_past = tf_msg + return + + # 把持対象が3秒以上停止している場合ピッキング動作開始 + if tf_stop_time < STOP_TIME_THRESHOLD: + return + + self.picking(tf_msg.transform.translation) def picking(self, target_position): - # 1. 掴み動作の準備とターゲットへの正対 + # 何かを掴んでいた時のためにハンドを開閉 self.move_gripper_angle(self.GRIPPER_OPEN) x = target_position.x y = target_position.y - theta_rad = math.atan2(y, x) - theta_deg = math.degrees(theta_rad) + theta_deg = math.degrees(math.atan2(y, x)) - self.move_arm_to_pose(0.0, 0.0, 0.3, 0, 0, theta_deg) + # ターゲットの正面に向ける + self.control_arm(0.0, 0.0, self.STANDBY[2], 0, 0, theta_deg) - # 2. ターゲット位置へのアプローチと掴み動作 - if not self.move_arm_to_pose(x, y, 0.04, 0, 90.0, theta_deg): - # アーム動作に失敗した場合は初期姿勢に戻る - self.move_arm_to_pose(*self.STANDBY_POSITION) + # ピック動作（掴みに行く） + if not self.control_arm(x, y, self.GRASP_HEIGHT, 0, self.GRASP_PITCH, theta_deg): + # アーム動作に失敗した場合は待機姿勢に戻る + self.control_arm(*self.STANDBY) return self.move_gripper_angle(self.GRIPPER_CLOSE) - # 3. 搬送および配置動作 - self.move_arm_to_pose(*self.TRANSIT_POSE_1) - self.move_arm_to_pose(*self.TRANSIT_POSE_2) - self.move_arm_to_pose(*self.PLACE_POSE) - + # プレース動作（移動して置く） + self.control_arm(*self.TRANSIT_1) + self.control_arm(*self.TRANSIT_2) + self.control_arm(*self.PLACE) self.move_gripper_angle(self.GRIPPER_OPEN) - # 4. 待機姿勢への復帰 - self.move_arm_to_pose(*self.PLACE_RETRACT_POSE) - self.move_arm_to_pose(*self.STANDBY_POSITION) + # 待機姿勢に戻る + self.control_arm(*self.PLACE_RETRACT) + self.control_arm(*self.STANDBY) self.move_gripper_angle(self.GRIPPER_DEFAULT) def move_gripper_angle(self, angle): + # グリッパを角度[rad]を指定して開閉する self.gripper.set_start_state_to_current_state() robot_state = RobotState(self.robot_model) robot_state.set_joint_group_positions('gripper', [angle]) self.gripper.set_goal_state(robot_state=robot_state) plan_and_execute( - self.crane_plus, - self.gripper, - self.logger, - single_plan_parameters=self.gripper_plan_request_params, + self.crane_plus, self.gripper, self.logger, + single_plan_parameters=self.gripper_plan_params, ) - def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): - # 指定された位置姿勢（x, y, z [m]、roll, pitch, yaw [deg]）へアームを移動する + def control_arm(self, x, y, z, roll, pitch, yaw): + # アームを目標位置（x, y, z [m]）・姿勢（roll, pitch, yaw [deg]）に動かす + # （IKの精度を向上させるため、位置・姿勢の許容範囲を設定） POSITION_TOLERANCE = 0.00001 ORIENTATION_TOLERANCE = 0.0001 @@ -256,12 +239,37 @@ def move_arm_to_pose(self, x, y, z, roll, pitch, yaw): self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) return plan_and_execute( - self.crane_plus, - self.arm, - self.logger, - single_plan_parameters=self.arm_plan_request_params, + self.crane_plus, self.arm, self.logger, + single_plan_parameters=self.arm_plan_params, ) + def set_constraints(self): + # アームの関節の一部に可動制限を設定する + constraints = Constraints() + constraints.name = 'arm_constraints' + + joint_constraint = JointConstraint() + joint_constraint.joint_name = 'crane_plus_joint1' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(100) + joint_constraint.tolerance_below = math.radians(100) + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + joint_constraint = JointConstraint() + joint_constraint.joint_name = 'crane_plus_joint3' + joint_constraint.position = 0.0 + joint_constraint.tolerance_above = math.radians(0) + joint_constraint.tolerance_below = math.radians(180) + joint_constraint.weight = 1.0 + constraints.joint_constraints.append(joint_constraint) + + self.arm.set_path_constraints(constraints) + + def clear_constraints(self): + # 設定された関節可動制限をクリアする + self.arm.clear_path_constraints() + def main(args=None): rclpy.init(args=args) @@ -270,6 +278,8 @@ def main(args=None): rclpy.spin(pick_and_place_tf_node) + pick_and_place_tf_node.clear_constraints() + # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 pick_and_place_tf_node.destroy_node() From d45306c4cb450419db529238ffe9c1591c6a3db9 Mon Sep 17 00:00:00 2001 From: motty Date: Wed, 1 Jul 2026 10:24:37 +0900 Subject: [PATCH 6/8] =?UTF-8?q?crane=5Fplus=5Fexamples=5Fpy:=20aruco/color?= =?UTF-8?q?=E6=A4=9C=E5=87=BA=E3=83=8E=E3=83=BC=E3=83=89=E3=82=92crane=5Fx?= =?UTF-8?q?7=E5=BD=A2=E5=BC=8F=E3=81=AB=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - aruco_detection: message_filtersのTimeSynchronizerで画像とカメラ情報を同期受信 marker_dictをコールバック内の毎回生成からコンストラクタのインスタンス変数に変更 - color_detection: 同様にmessage_filtersで2トピックを同期、camera_info_callbackを廃止 d_area <= 10000の早期リターンパターンをcrane_x7に合わせて統一 - package.xml: cv_bridge, geometry_msgs, image_geometry, message_filters, python3-opencv, sensor_msgs, tf2_rosの不足dependを追加 Co-Authored-By: Claude Sonnet 4.6 --- .../crane_plus_examples_py/aruco_detection.py | 49 ++++----- .../crane_plus_examples_py/color_detection.py | 100 ++++++++---------- crane_plus_examples_py/package.xml | 11 +- 3 files changed, 79 insertions(+), 81 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index b43f77be..c2d6cde5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -16,6 +16,7 @@ from cv2 import aruco from cv_bridge import CvBridge from geometry_msgs.msg import TransformStamped +import message_filters import numpy as np import rclpy from rclpy.node import Node @@ -27,44 +28,42 @@ class ImageSubscriber(Node): def __init__(self): super().__init__('aruco_detection') - self.image_subscription = self.create_subscription( - Image, 'image_raw', self.image_callback, 10 - ) - self.camera_info_subscription = self.create_subscription( - CameraInfo, 'camera_info', self.camera_info_callback, 10 - ) - self.tf_broadcaster = TransformBroadcaster(self) + self.image_sub = message_filters.Subscriber(self, Image, 'image_raw') + self.info_sub = message_filters.Subscriber(self, CameraInfo, 'camera_info') + self.ts = message_filters.TimeSynchronizer([self.image_sub, self.info_sub], 10) + self.ts.registerCallback(self.camera_callback) - self.camera_info = None + # ArUcoマーカのデータセットを読み込む + # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + self.marker_dict = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) + self.tf_broadcaster = TransformBroadcaster(self) self.bridge = CvBridge() - def image_callback(self, msg): + def camera_callback(self, img_msg, info_msg): # 画像データをROSのメッセージからOpenCVの配列に変換 - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding=img_msg.encoding) cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) - if not self.camera_info: - return - - # ArUcoマーカのデータセットを読み込む - # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの - MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) - # マーカID - ids = [] # 画像座標系上のマーカ頂点位置 corners = [] + # マーカID + ids = [] # マーカの検出 - corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + corners, ids, _ = aruco.detectMarkers(cv_img, self.marker_dict) + if ids is None: return # マーカの検出数 n_markers = len(ids) + # カメラパラメータ - CAMERA_MATRIX = np.array(self.camera_info.k).reshape(3, 3) - DIST_COEFFS = np.array(self.camera_info.d).reshape(1, 5) + CAMERA_MATRIX = np.array(info_msg.k).reshape(3, 3) + DIST_COEFFS = np.array(info_msg.d).reshape(1, 5) + # マーカ一辺の長さ 0.04 [m] MARKER_LENGTH = 0.04 + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS @@ -72,13 +71,13 @@ def image_callback(self, msg): # マーカの位置姿勢をtfで配信 for i in range(n_markers): - # tfの配信 t = TransformStamped() - t.header = msg.header + t.header = img_msg.header t.child_frame_id = 'target_' + str(ids[i][0]) t.transform.translation.x = tvecs[i][0][0] t.transform.translation.y = tvecs[i][0][1] t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトルをクォータニオンに変換 marker_orientation_rot = Rotation.from_rotvec(rvecs[i][0]) marker_orientation_quat = marker_orientation_rot.as_quat() @@ -86,10 +85,8 @@ def image_callback(self, msg): t.transform.rotation.y = marker_orientation_quat[1] t.transform.rotation.z = marker_orientation_quat[2] t.transform.rotation.w = marker_orientation_quat[3] - self.tf_broadcaster.sendTransform(t) - def camera_info_callback(self, msg): - self.camera_info = msg + self.tf_broadcaster.sendTransform(t) def main(args=None): diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index bf36f639..6fd88e33 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -16,6 +16,7 @@ from cv_bridge import CvBridge from geometry_msgs.msg import TransformStamped from image_geometry import PinholeCameraModel +import message_filters import rclpy from rclpy.node import Node from sensor_msgs.msg import CameraInfo, Image @@ -25,22 +26,16 @@ class ImageSubscriber(Node): def __init__(self): super().__init__('color_detection') - self.image_subscription = self.create_subscription( - Image, 'image_raw', self.image_callback, 10 - ) - self.camera_info_subscription = self.create_subscription( - CameraInfo, 'camera_info', self.camera_info_callback, 10 - ) + self.color_sub = message_filters.Subscriber(self, Image, 'image_raw') + self.info_sub = message_filters.Subscriber(self, CameraInfo, 'camera_info') + self.sync = message_filters.TimeSynchronizer([self.color_sub, self.info_sub], 10) + self.sync.registerCallback(self.camera_callback) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) self.tf_broadcaster = TransformBroadcaster(self) - self.camera_info = None self.bridge = CvBridge() - def image_callback(self, msg): - # カメラのパラメータを取得してから処理を行う - if not self.camera_info: - return - + def camera_callback(self, img_msg, info_msg): # 赤い物体を検出するようにHSVの範囲を設定 # 周囲の明るさ等の動作環境に合わせて調整 LOW_H_1 = 0 @@ -52,10 +47,10 @@ def image_callback(self, msg): LOW_V = 100 HIGH_V = 255 - # ウェブカメラの画像を受け取る - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + # カメラ画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding=img_msg.encoding) - # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） + # 画像をRGBからHSVに変換 cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) # 画像の二値化 @@ -78,48 +73,47 @@ def image_callback(self, msg): d_m10 = moment['m10'] d_area = moment['m00'] - # 検出した領域のピクセル数が10000より大きい場合 - if d_area > 10000: - # カメラモデル作成 - camera_model = PinholeCameraModel() - - # カメラのパラメータを設定 - camera_model.fromCameraInfo(self.camera_info) - - # 画像座標系における把持対象物の位置（2D） - pixel_x = d_m10 / d_area - pixel_y = d_m01 / d_area - point = (pixel_x, pixel_y) - - # 補正後の画像座標系における把持対象物の位置を取得（2D） - rect_point = camera_model.rectifyPoint(point) - - # カメラ座標系から見た把持対象物の方向（Ray）を取得する - ray = camera_model.projectPixelTo3dRay(rect_point) - - # カメラの高さを0.46[m]として把持対象物の位置を計算 - CAMERA_HEIGHT = 0.46 - object_position = [ - ray[0] * CAMERA_HEIGHT, - ray[1] * CAMERA_HEIGHT, - ray[2] * CAMERA_HEIGHT, - ] - - # 把持対象物の位置をTFに配信 - t = TransformStamped() - t.header = msg.header - t.child_frame_id = 'target_0' - t.transform.translation.x = object_position[0] - t.transform.translation.y = object_position[1] - t.transform.translation.z = object_position[2] - self.tf_broadcaster.sendTransform(t) - # 閾値による二値化画像を配信 img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') self.image_thresholded_publisher.publish(img_thresholded_msg) - def camera_info_callback(self, msg): - self.camera_info = msg + # 検出した領域のピクセル数が10000より大きい場合に把持位置を配信 + if d_area <= 10000: + return + + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(info_msg) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyPoint(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # カメラの高さを0.46[m]として把持対象物の位置を計算 + CAMERA_HEIGHT = 0.46 + object_position = [ + ray[0] * CAMERA_HEIGHT, + ray[1] * CAMERA_HEIGHT, + ray[2] * CAMERA_HEIGHT, + ] + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = img_msg.header + t.child_frame_id = 'target_0' + t.transform.translation.x = object_position[0] + t.transform.translation.y = object_position[1] + t.transform.translation.z = object_position[2] + self.tf_broadcaster.sendTransform(t) def main(args=None): diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index 5b37710e..5bc535fc 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -11,12 +11,19 @@ Atsushi Kuwagata Yusuke Kato - rclpy - std_msgs + cv_bridge + geometry_msgs + image_geometry + message_filters moveit moveit_py python3-numpy + python3-opencv python3-scipy + rclpy + sensor_msgs + std_msgs + tf2_ros ament_copyright ament_flake8 From 245dbc63653723db02864e7c0d12fff2d7928850 Mon Sep 17 00:00:00 2001 From: motty Date: Wed, 1 Jul 2026 11:51:53 +0900 Subject: [PATCH 7/8] =?UTF-8?q?crane=5Fplus=5Fexamples:=20aruco/color?= =?UTF-8?q?=E6=A4=9C=E5=87=BA=E3=83=8E=E3=83=BC=E3=83=89=E3=82=92crane=5Fx?= =?UTF-8?q?7=E5=BD=A2=E5=BC=8F=E3=81=AB=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - aruco_detection: image_transport::create_camera_subscriptionで画像とカメラ情報を同期受信 marker_dict_をコールバック内の毎回生成からコンストラクタのメンバ変数に変更 クォータニオン変換をtf2::Matrix3x3+Rodriguesからcv::Quatd::createFromRvecに簡略化 - color_detection: message_filtersのExactTimeで2トピックを同期、camera_info_callbackを廃止 d_area <= 10000の早期リターンパターンをcrane_x7に合わせて統一 - CMakeLists.txt: angles, image_transport, message_filtersのfind_packageと依存を追加 - package.xml: angles, image_transport, message_filtersの不足dependを追加 Co-Authored-By: Claude Sonnet 4.6 --- crane_plus_examples/CMakeLists.txt | 10 +- crane_plus_examples/package.xml | 5 +- crane_plus_examples/src/aruco_detection.cpp | 124 +++++------ crane_plus_examples/src/color_detection.cpp | 226 ++++++++++---------- 4 files changed, 180 insertions(+), 185 deletions(-) diff --git a/crane_plus_examples/CMakeLists.txt b/crane_plus_examples/CMakeLists.txt index 421bb586..7f4906f6 100644 --- a/crane_plus_examples/CMakeLists.txt +++ b/crane_plus_examples/CMakeLists.txt @@ -17,12 +17,15 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(message_filters REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(image_geometry REQUIRED) # Build and install node executables set(executable_list @@ -37,13 +40,16 @@ set(executable_list foreach(loop_var IN LISTS executable_list) add_executable(${loop_var} src/${loop_var}.cpp) ament_target_dependencies(${loop_var} + angles cv_bridge geometry_msgs + image_geometry + image_transport + message_filters moveit_ros_planning_interface OpenCV rclcpp tf2_geometry_msgs - image_geometry ) install(TARGETS diff --git a/crane_plus_examples/package.xml b/crane_plus_examples/package.xml index 1232d806..9cbcde9d 100644 --- a/crane_plus_examples/package.xml +++ b/crane_plus_examples/package.xml @@ -13,17 +13,20 @@ ament_cmake + angles crane_plus_control crane_plus_description crane_plus_moveit_config cv_bridge geometry_msgs + image_geometry + image_transport libopencv-dev + message_filters moveit_ros_planning_interface rclcpp tf2_geometry_msgs usb_cam - image_geometry ament_lint_auto ament_lint_common diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index bcdb626a..dbc290fd 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -16,7 +16,6 @@ // https://docs.opencv.org/4.2.0/d5/dae/tutorial_aruco_detection.html // https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html -#include #include #include #include @@ -27,11 +26,13 @@ #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" #include "opencv2/aruco.hpp" +#include "opencv2/core/quaternion.hpp" #include "cv_bridge/cv_bridge.hpp" -#include "tf2/LinearMath/Quaternion.hpp" -#include "tf2/LinearMath/Matrix3x3.hpp" #include "tf2_ros/transform_broadcaster.h" +#include "image_transport/image_transport.hpp" +#include "image_transport/camera_subscriber.hpp" using std::placeholders::_1; +using std::placeholders::_2; class ImageSubscriber : public rclcpp::Node { @@ -39,88 +40,71 @@ class ImageSubscriber : public rclcpp::Node ImageSubscriber() : Node("aruco_detection") { - image_subscription_ = this->create_subscription( - "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); + camera_subscription_ = image_transport::create_camera_subscription( + this, + "image_raw", + std::bind(&ImageSubscriber::camera_callback, this, _1, _2), + "raw"); - camera_info_subscription_ = this->create_subscription( - "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); + // ArUcoマーカのデータセットを読み込む + // DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + marker_dict_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50); tf_broadcaster_ = std::make_unique(*this); } private: - rclcpp::Subscription::SharedPtr image_subscription_; - rclcpp::Subscription::SharedPtr camera_info_subscription_; - sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; + image_transport::CameraSubscriber camera_subscription_; std::unique_ptr tf_broadcaster_; + cv::Ptr marker_dict_; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + void camera_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & img_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) { - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + auto cv_img = cv_bridge::toCvShare(img_msg, img_msg->encoding); cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2BGR); - if (camera_info_) { - // ArUcoマーカのデータセットを読み込む - // DICT_6x6_50は6x6ビットのマーカが50個収録されたもの - const auto MARKER_DICT = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50); - // マーカID - std::vector ids; - // 画像座標系上のマーカ頂点位置 - std::vector> corners; - // マーカの検出 - cv::aruco::detectMarkers(cv_img->image, MARKER_DICT, corners, ids); - // マーカの検出数 - int n_markers = ids.size(); - // カメラパラメータ - const auto CAMERA_MATRIX = cv::Mat(3, 3, CV_64F, camera_info_->k.data()); - const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, camera_info_->d.data()); - // マーカ一辺の長さ 0.04 [m] - const float MARKER_LENGTH = 0.04; + // マーカID + std::vector ids; + // 画像座標系上のマーカ頂点位置 + std::vector> corners; + // マーカの検出 + cv::aruco::detectMarkers(cv_img->image, marker_dict_, corners, ids); + // マーカの検出数 + int n_markers = ids.size(); - // マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 - if (n_markers > 0) { - // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 - std::vector rvecs, tvecs; - cv::aruco::estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs); - - for (int i = 0; i < n_markers; i++) { - // tfの配信 - geometry_msgs::msg::TransformStamped t; - t.header = msg->header; - t.child_frame_id = "target_" + std::to_string(ids[i]); - t.transform.translation.x = tvecs[i][0]; - t.transform.translation.y = tvecs[i][1]; - t.transform.translation.z = tvecs[i][2]; - // 回転ベクトルをクォータニオンに変換 - tf2::Quaternion q; - cv::Mat cv_rotation_matrix; - cv::Rodrigues(rvecs[i], cv_rotation_matrix); - tf2::Matrix3x3 tf2_rotation_matrix = tf2::Matrix3x3( - cv_rotation_matrix.at(0, 0), - cv_rotation_matrix.at(0, 1), - cv_rotation_matrix.at(0, 2), - cv_rotation_matrix.at(1, 0), - cv_rotation_matrix.at(1, 1), - cv_rotation_matrix.at(1, 2), - cv_rotation_matrix.at(2, 0), - cv_rotation_matrix.at(2, 1), - cv_rotation_matrix.at(2, 2)); - tf2_rotation_matrix.getRotation(q); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); - tf_broadcaster_->sendTransform(t); - } - } + if (n_markers <= 0) { + return; } - } - void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) - { - camera_info_ = msg; + // カメラパラメータの読み込み + const auto CAMERA_MATRIX = cv::Mat(3, 3, CV_64F, const_cast(info_msg->k.data())); + const auto DIST_COEFFS = cv::Mat(1, 5, CV_64F, const_cast(info_msg->d.data())); + // マーカ一辺の長さ 0.04 [m] + const float MARKER_LENGTH = 0.04; + // マーカの回転ベクトルと位置ベクトル + std::vector rvecs, tvecs; + // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + cv::aruco::estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs); + + // tfの配信 + for (int i = 0; i < n_markers; i++) { + geometry_msgs::msg::TransformStamped t; + t.header = img_msg->header; + t.child_frame_id = "target_" + std::to_string(ids[i]); + t.transform.translation.x = tvecs[i][0]; + t.transform.translation.y = tvecs[i][1]; + t.transform.translation.z = tvecs[i][2]; + cv::Quatd cv_q = cv::Quatd::createFromRvec(rvecs[i]); + t.transform.rotation.x = cv_q.x; + t.transform.rotation.y = cv_q.y; + t.transform.rotation.z = cv_q.z; + t.transform.rotation.w = cv_q.w; + tf_broadcaster_->sendTransform(t); + } } }; diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp index 33e73894..d22ff0f4 100644 --- a/crane_plus_examples/src/color_detection.cpp +++ b/crane_plus_examples/src/color_detection.cpp @@ -15,23 +15,25 @@ // Reference: // https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html -#include -#include -#include #include #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" -#include "tf2/LinearMath/Quaternion.hpp" -#include "tf2/LinearMath/Matrix3x3.hpp" #include "tf2_ros/transform_broadcaster.h" #include "opencv2/opencv.hpp" #include "opencv2/imgproc/imgproc.hpp" #include "cv_bridge/cv_bridge.hpp" #include "image_geometry/pinhole_camera_model.hpp" +#include "image_transport/image_transport.hpp" +#include "image_transport/subscriber_filter.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/exact_time.h" + using std::placeholders::_1; +using std::placeholders::_2; class ImageSubscriber : public rclcpp::Node { @@ -39,11 +41,12 @@ class ImageSubscriber : public rclcpp::Node ImageSubscriber() : Node("color_detection") { - image_subscription_ = this->create_subscription( - "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); + color_sub_.subscribe(this, "image_raw", "raw"); + info_sub_.subscribe(this, "camera_info"); - camera_info_subscription_ = this->create_subscription( - "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); + sync_ = std::make_unique>( + ExactPolicy(10), color_sub_, info_sub_); + sync_->registerCallback(std::bind(&ImageSubscriber::camera_callback, this, _1, _2)); image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); @@ -53,115 +56,114 @@ class ImageSubscriber : public rclcpp::Node } private: - rclcpp::Subscription::SharedPtr image_subscription_; - rclcpp::Subscription::SharedPtr camera_info_subscription_; + using ExactPolicy = message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo>; + image_transport::SubscriberFilter color_sub_; + message_filters::Subscriber info_sub_; + std::unique_ptr> sync_; rclcpp::Publisher::SharedPtr image_thresholded_publisher_; - sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; std::unique_ptr tf_broadcaster_; - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + void camera_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & img_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) { - // カメラのパラメータを取得してから処理を行う - if (camera_info_) { - // 赤い物体を検出するようにHSVの範囲を設定 - // 周囲の明るさ等の動作環境に合わせて調整 - const int LOW_H_1 = 0, HIGH_H_1 = 10; - const int LOW_H_2 = 170, HIGH_H_2 = 179; - const int LOW_S = 100, HIGH_S = 255; - const int LOW_V = 100, HIGH_V = 255; - - // ウェブカメラの画像を受け取る - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） - cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); - - // 画像処理用の変数を用意 - cv::Mat img_mask_1; - cv::Mat img_mask_2; - cv::Mat img_thresholded; - - // 画像の二値化 - cv::inRange( - cv_img->image, - cv::Scalar(LOW_H_1, LOW_S, LOW_V), - cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), - img_mask_1); - cv::inRange( - cv_img->image, - cv::Scalar(LOW_H_2, LOW_S, LOW_V), - cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), - img_mask_2); - - // マスク画像の合成 - img_thresholded = img_mask_1 | img_mask_2; - - // ノイズ除去の処理 - cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_OPEN, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - - // 穴埋めの処理 - cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_CLOSE, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); - - // 画像の検出領域におけるモーメントを計算 - cv::Moments moment = moments(img_thresholded); - double d_m01 = moment.m01; - double d_m10 = moment.m10; - double d_area = moment.m00; - - // 検出した領域のピクセル数が10000より大きい場合 - if (d_area > 10000) { - // カメラモデル作成 - image_geometry::PinholeCameraModel camera_model; - - // カメラのパラメータを設定 - camera_model.fromCameraInfo(camera_info_); - - // 画像座標系における把持対象物の位置（2D） - const double pixel_x = d_m10 / d_area; - const double pixel_y = d_m01 / d_area; - const cv::Point2d point(pixel_x, pixel_y); - - // 補正後の画像座標系における把持対象物の位置を取得（2D） - const cv::Point2d rect_point = camera_model.rectifyPoint(point); - - // カメラ座標系から見た把持対象物の方向（Ray）を取得する - const cv::Point3d ray = camera_model.projectPixelTo3dRay(rect_point); - - // カメラの高さを0.44[m]として把持対象物の位置を計算 - const double CAMERA_HEIGHT = 0.46; - cv::Point3d object_position( - ray.x * CAMERA_HEIGHT, - ray.y * CAMERA_HEIGHT, - ray.z * CAMERA_HEIGHT); - - // 把持対象物の位置をTFに配信 - geometry_msgs::msg::TransformStamped t; - t.header = msg->header; - t.child_frame_id = "target_0"; - t.transform.translation.x = object_position.x; - t.transform.translation.y = object_position.y; - t.transform.translation.z = object_position.z; - tf_broadcaster_->sendTransform(t); - } - - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = - cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); - image_thresholded_publisher_->publish(*img_thresholded_msg); + // 赤い物体を検出するようにHSVの範囲を設定 + // 周囲の明るさ等の動作環境に合わせて調整 + const int LOW_H_1 = 0, HIGH_H_1 = 10; + const int LOW_H_2 = 170, HIGH_H_2 = 179; + const int LOW_S = 100, HIGH_S = 255; + const int LOW_V = 100, HIGH_V = 255; + + // カメラ画像を受け取る + auto cv_img = cv_bridge::toCvShare(img_msg, img_msg->encoding); + + // 画像をRGBからHSVに変換 + cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); + + // 画像処理用の変数を用意 + cv::Mat img_mask_1; + cv::Mat img_mask_2; + cv::Mat img_thresholded; + + // 画像の二値化 + cv::inRange( + cv_img->image, + cv::Scalar(LOW_H_1, LOW_S, LOW_V), + cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), + img_mask_1); + cv::inRange( + cv_img->image, + cv::Scalar(LOW_H_2, LOW_S, LOW_V), + cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), + img_mask_2); + + // マスク画像の合成 + img_thresholded = img_mask_1 | img_mask_2; + + // ノイズ除去の処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 穴埋めの処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_CLOSE, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 画像の検出領域におけるモーメントを計算 + cv::Moments moment = moments(img_thresholded); + double d_m01 = moment.m01; + double d_m10 = moment.m10; + double d_area = moment.m00; + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = + cv_bridge::CvImage(img_msg->header, "mono8", img_thresholded).toImageMsg(); + image_thresholded_publisher_->publish(*img_thresholded_msg); + + // 検出した領域のピクセル数が10000より大きい場合に把持位置を配信 + if (d_area <= 10000) { + return; } - } - void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) - { - camera_info_ = msg; + // カメラモデル作成 + image_geometry::PinholeCameraModel camera_model; + + // カメラのパラメータを設定 + camera_model.fromCameraInfo(*info_msg); + + // 画像座標系における把持対象物の位置（2D） + const double pixel_x = d_m10 / d_area; + const double pixel_y = d_m01 / d_area; + const cv::Point2d point(pixel_x, pixel_y); + + // 補正後の画像座標系における把持対象物の位置を取得（2D） + const cv::Point2d rect_point = camera_model.rectifyPoint(point); + + // カメラ座標系から見た把持対象物の方向（Ray）を取得する + const cv::Point3d ray = camera_model.projectPixelTo3dRay(rect_point); + + // カメラの高さを0.46[m]として把持対象物の位置を計算 + const double CAMERA_HEIGHT = 0.46; + cv::Point3d object_position( + ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT); + + // 把持対象物の位置をTFに配信 + geometry_msgs::msg::TransformStamped t; + t.header = img_msg->header; + t.child_frame_id = "target_0"; + t.transform.translation.x = object_position.x; + t.transform.translation.y = object_position.y; + t.transform.translation.z = object_position.z; + tf_broadcaster_->sendTransform(t); } }; From d09897f359a93e4e384bb1730ea1c0eb775456d3 Mon Sep 17 00:00:00 2001 From: motty Date: Wed, 1 Jul 2026 11:53:04 +0900 Subject: [PATCH 8/8] =?UTF-8?q?crane=5Fplus=5Fexamples=5Fpy:=20utils.py?= =?UTF-8?q?=E3=82=92crane=5Fx7=E5=BD=A2=E5=BC=8F=E3=81=AB=E7=B5=B1?= =?UTF-8?q?=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - ライセンスヘッダを標準書式（各行間に空行あり）に修正 - コメントを英語から日本語に統一（「軌道を計画する」「計画した軌道を実行する」） Co-Authored-By: Claude Sonnet 4.6 --- crane_plus_examples_py/crane_plus_examples_py/utils.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index e97753af..35a9e47b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,8 +1,11 @@ # Copyright 2025 RT Corporation +# # 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. @@ -20,7 +23,7 @@ def plan_and_execute( multi_plan_parameters=None, sleep_time=0.0, ): - # plan to goal + # 軌道を計画する logger.info('Planning trajectory') if multi_plan_parameters is not None: plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters) @@ -29,7 +32,7 @@ def plan_and_execute( else: plan_result = planning_component.plan() - # execute the plan + # 計画した軌道を実行する if plan_result: logger.info('Executing plan') robot_trajectory = plan_result.trajectory