diff --git a/crane_plus_examples/CMakeLists.txt b/crane_plus_examples/CMakeLists.txt
index 421bb58..7f4906f 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 1232d80..9cbcde9 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 77ca11d..dbc290f 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) {
- for (int i = 0; i < n_markers; i++) {
- // マーカの回転ベクトルと位置ベクトル
- std::vector rvecs, tvecs;
- // 画像座標系上のマーカ位置を三次元のカメラ座標系に変換
- cv::aruco::estimatePoseSingleMarkers(
- corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS, rvecs, tvecs);
-
- // 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 33e7389..d22ff0f 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);
}
};
diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp
index 66cea24..5216326 100644
--- a/crane_plus_examples/src/gripper_control.cpp
+++ b/crane_plus_examples/src/gripper_control.cpp
@@ -16,49 +16,66 @@
// 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;
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control");
-
-double to_radians(const double deg_angle)
+class GripperControl
{
- return deg_angle * M_PI / 180.0;
-}
+public:
+ // グリッパの開閉角度
+ 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を初期化する
+ explicit GripperControl(rclcpp::Node::SharedPtr node)
+ {
+ 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
+ }
+
+ // グリッパを角度[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();
+ }
+
+private:
+ 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_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();
+ auto node = rclcpp::Node::make_shared("gripper_control", node_options);
- 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
+ // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる
+ std::thread spin_thread([node]() {rclcpp::spin(node);});
- auto gripper_joint_values = move_group_gripper.getCurrentJointValues();
+ GripperControl controller(node);
- gripper_joint_values[0] = to_radians(30);
- move_group_gripper.setJointValueTarget(gripper_joint_values);
- move_group_gripper.move();
+ // グリッパを閉じる
+ controller.move_gripper_angle(GripperControl::GRIPPER_CLOSE);
- gripper_joint_values[0] = to_radians(-30);
- move_group_gripper.setJointValueTarget(gripper_joint_values);
- move_group_gripper.move();
+ // グリッパを開く
+ controller.move_gripper_angle(GripperControl::GRIPPER_OPEN);
- gripper_joint_values[0] = to_radians(0);
- move_group_gripper.setJointValueTarget(gripper_joint_values);
- move_group_gripper.move();
+ // グリッパを0度にする
+ controller.move_gripper_angle(GripperControl::GRIPPER_DEFAULT);
+ // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する
rclcpp::shutdown();
+ spin_thread.join();
return 0;
}
diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp
index 609bc4b..61fff47 100644
--- a/crane_plus_examples/src/joint_values.cpp
+++ b/crane_plus_examples/src/joint_values.cpp
@@ -16,49 +16,78 @@
// 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;
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("joint_values");
-
-double to_radians(const double deg_angle)
+class JointValues
{
- return deg_angle * M_PI / 180.0;
-}
+public:
+ // ノードを受け取り、アームのMoveGroupInterfaceを初期化する
+ explicit JointValues(rclcpp::Node::SharedPtr node)
+ {
+ 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 move_arm_to_named_pose(const std::string & name)
+ {
+ move_group_arm_->setNamedTarget(name);
+ move_group_arm_->move();
+ }
+
+ // 各ジョイント角度[rad]を指定してアームを動かす
+ void move_arm_joint_values(const std::vector & joint_values)
+ {
+ move_group_arm_->setJointValueTarget(joint_values);
+ move_group_arm_->move();
+ }
+
+ // アームの現在のジョイント角度を取得する
+ std::vector get_current_arm_joint_values()
+ {
+ return move_group_arm_->getCurrentJointValues();
+ }
+
+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_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();
+ auto node = rclcpp::Node::make_shared("joint_values", node_options);
+
+ // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる
+ std::thread spin_thread([node]() {rclcpp::spin(node);});
- 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
+ JointValues controller(node);
- move_group_arm.setNamedTarget("vertical");
- move_group_arm.move();
+ // verticalの姿勢から開始
+ controller.move_arm_to_named_pose("vertical");
- auto joint_values = move_group_arm.getCurrentJointValues();
- double target_joint_value = to_radians(45.0);
+ // 各関節を順番に45度に動かす
+ 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_joint_value;
- move_group_arm.setJointValueTarget(joint_values);
- move_group_arm.move();
+ joint_values[i] = TARGET_ANGLE;
+ controller.move_arm_joint_values(joint_values);
}
- move_group_arm.setNamedTarget("vertical");
- move_group_arm.move();
+ // verticalの姿勢に戻る
+ controller.move_arm_to_named_pose("vertical");
+ // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する
rclcpp::shutdown();
+ 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 c5d5116..f9de2b6 100644
--- a/crane_plus_examples/src/pick_and_place.cpp
+++ b/crane_plus_examples/src/pick_and_place.cpp
@@ -16,124 +16,157 @@
// 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;
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place");
-
-double to_radians(const double deg_angle)
+class PickAndPlace
{
- return deg_angle * M_PI / 180.0;
-}
+public:
+ // グリッパの開閉角度
+ 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を初期化する
+ explicit PickAndPlace(rclcpp::Node::SharedPtr node)
+ {
+ 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(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 move_arm_to_named_pose(const std::string & name)
+ {
+ move_group_arm_->setNamedTarget(name);
+ move_group_arm_->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]）に動かす
+ 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 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_arm_to_pose(target_pose);
+ }
+
+ // グリッパを角度[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();
+ }
+
+private:
+ 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 node = rclcpp::Node::make_shared("pick_and_place", node_options);
+
+ // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる
+ 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.move_arm_to_named_pose("vertical");
+ controller.move_gripper_angle(PickAndPlace::GRIPPER_DEFAULT);
+
+ // ピック準備
+ 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.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);
+
+ // プレース動作
+ // 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(PickAndPlace::GRIPPER_DEFAULT);
+
+ // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する
rclcpp::shutdown();
+ 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 f62dd58..fcff462 100644
--- a/crane_plus_examples/src/pick_and_place_tf.cpp
+++ b/crane_plus_examples/src/pick_and_place_tf.cpp
@@ -24,29 +24,52 @@
#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;
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")
{
- using namespace std::placeholders;
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 +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();
+ }
- // 可動範囲を制限する
+ // アームの関節の一部に可動制限を設定する
+ void set_constraints()
+ {
moveit_msgs::msg::Constraints constraints;
constraints.name = "arm_constraints";
@@ -83,23 +164,18 @@ class PickAndPlaceTf : public rclcpp::Node
constraints.joint_constraints.push_back(joint_constraint);
move_group_arm_->setPathConstraints(constraints);
+ }
- // 待機姿勢
- control_arm(0.0, 0.0, 0.3, 0, 0, 0);
-
- tf_buffer_ =
- std::make_unique(this->get_clock());
- tf_listener_ =
- std::make_shared(*tf_buffer_);
-
- timer_ = this->create_wall_timer(
- 500ms, std::bind(&PickAndPlaceTf::on_timer, this));
+ // 設定された関節可動制限をクリアする
+ void clear_constraints()
+ {
+ move_group_arm_->clearPathConstraints();
}
private:
void on_timer()
{
- // target_0のtf位置姿勢を取得
+ // target_0（把持対象）のTFを取得
geometry_msgs::msg::TransformStamped tf_msg;
try {
@@ -114,104 +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();
+ 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;
+
+ tf2::Stamped tf_current;
+ tf2::convert(tf_msg, tf_current);
+
+ 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秒以内に受け取った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_past_ = tf;
- }
+ if (tf_elapsed_time > FILTERING_TIME) {
+ return;
}
+
+ double tf_diff = (tf_past_.getOrigin() - tf_current.getOrigin()).length();
+
+ // 把持対象の位置が停止していることを判定
+ if (tf_diff > DISTANCE_THRESHOLD) {
+ tf_past_ = tf_current;
+ return;
+ }
+
+ // 把持対象が3秒以上停止している場合ピッキング動作開始
+ if (tf_stop_time < STOP_TIME_THRESHOLD) {
+ return;
+ }
+
+ // 把持対象が低すぎる場合は把持位置を調整
+ if (tf_current.getOrigin().z() < TARGET_Z_MIN_LIMIT) {
+ tf_current.getOrigin().setZ(TARGET_Z_MIN_LIMIT);
+ }
+
+ picking(tf_current.getOrigin());
}
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);
-
- // 何かを掴んでいた時のためにハンドを開く
- control_gripper(GRIPPER_OPEN);
+ // 何かを掴んでいた時のためにハンドを開閉
+ move_gripper_angle(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;
+ double theta_deg = std::atan2(y, x) * 180.0 / M_PI;
- // 把持対象物に正対する
- control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg);
+ // ターゲットの正面に向ける
+ control_arm(0.0, 0.0, STANDBY.z, 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);
+ // ピック動作（掴みに行く）
+ if (!control_arm(x, y, GRASP_HEIGHT, 0, GRASP_PITCH, theta_deg)) {
+ // アーム動作に失敗した場合は待機姿勢に戻る
+ control_arm(STANDBY);
return;
}
+ move_gripper_angle(GRIPPER_CLOSE);
- // ハンドを閉じる
- control_gripper(GRIPPER_CLOSE);
-
- // 移動する
- control_arm(0.12, 0.0, 0.17, 0, 90, 0);
-
- // 横を向く
- control_arm(0.0, -0.12, 0.17, 0, 90, -90);
-
- // 下ろす
- control_arm(0.0, -0.25, 0.05, 0, 90, -90);
-
- // ハンドを開く
- control_gripper(GRIPPER_OPEN);
-
- // 少しだけハンドを持ち上げる
- control_arm(0.0, -0.25, 0.10, 0, 90, -90);
+ // プレース動作（移動して置く）
+ control_arm(TRANSIT_1);
+ control_arm(TRANSIT_2);
+ control_arm(PLACE);
+ move_gripper_angle(GRIPPER_OPEN);
// 待機姿勢に戻る
- control_arm(0.0, 0.0, 0.3, 0, 0, 0);
-
- // ハンドを閉じる
- control_gripper(GRIPPER_DEFAULT);
- }
-
- // グリッパ制御
- void control_gripper(const double angle)
- {
- auto joint_values = move_group_gripper_->getCurrentJointValues();
- joint_values[0] = angle;
- move_group_gripper_->setJointValueTarget(joint_values);
- move_group_gripper_->move();
- }
-
- // アーム制御
- 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;
+ control_arm(PLACE_RETRACT);
+ control_arm(STANDBY);
+ move_gripper_angle(GRIPPER_DEFAULT);
}
std::shared_ptr move_group_arm_;
@@ -230,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 e5d4c79..0e4bb75 100644
--- a/crane_plus_examples/src/pose_groupstate.cpp
+++ b/crane_plus_examples/src/pose_groupstate.cpp
@@ -16,37 +16,54 @@
// 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 PoseGroupstate
+{
+public:
+ // ノードを受け取り、アームのMoveGroupInterfaceを初期化する
+ explicit PoseGroupstate(rclcpp::Node::SharedPtr node)
+ {
+ 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 move_arm_to_named_pose(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 node = rclcpp::Node::make_shared("pose_groupstate", node_options);
- move_group_arm.setNamedTarget("home");
- move_group_arm.move();
+ // MoveGroupInterfaceのデッドロックを防ぐため、スピン処理を別スレッドで走らせる
+ std::thread spin_thread([node]() {rclcpp::spin(node);});
- move_group_arm.setNamedTarget("vertical");
- move_group_arm.move();
+ PoseGroupstate controller(node);
- move_group_arm.setNamedTarget("home");
- move_group_arm.move();
+ // SRDFに定義されている名前付き姿勢を順番に実行する
+ controller.move_arm_to_named_pose("home");
+ controller.move_arm_to_named_pose("vertical");
+ controller.move_arm_to_named_pose("home");
+ // 終了処理: rclcppを終了したのち、バックグラウンドスレッドを安全に回収する
rclcpp::shutdown();
+ spin_thread.join();
return 0;
}
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 b43f77b..c2d6cde 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 bf36f63..6fd88e3 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/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py
index 3886700..9c2ef0d 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,52 @@
from rclpy.logging import get_logger
+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_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])
+ 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 = GripperControl()
+
+ # グリッパを閉じる
+ controller.move_gripper_angle(GripperControl.GRIPPER_CLOSE)
+
+ # グリッパを開く
+ controller.move_gripper_angle(GripperControl.GRIPPER_OPEN)
+
+ # グリッパを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 50eb734..4b8fed8 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 JointValues:
+ 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_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_arm_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 = JointValues()
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_arm_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_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 ec33167..433932c 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,270 +31,174 @@
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
-
-
-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
-
- # グリッパの開閉角
+class PickAndPlace:
+ # グリッパの開閉角度
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,
- )
+ 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')
+
+ 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):
+ # 位置姿勢の制約を設定してアームを動かす
+ # （IKの精度を向上させるため、位置・姿勢の許容範囲を設定）
+ 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)
+
+ 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 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])
+ 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,
+ )
- # 掴みに行く
- 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,
- )
+def main(args=None):
+ rclpy.init(args=args)
+ 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(PickAndPlace.GRIPPER_DEFAULT)
+
+ # ピック準備
+ 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.control_arm(PICK_X, PICK_Y, PICK_Z, PICK_ROLL, PICK_PITCH, PICK_YAW)
+ # 掴む
+ controller.move_gripper_angle(PickAndPlace.GRIPPER_CLOSE)
# 持ち上げる
- 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.control_arm(PICK_X, PICK_Y, LIFTING_HEIGHT, PICK_ROLL, PICK_PITCH, PICK_YAW)
+
+ # プレース動作
+ # 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(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 6dd60be..ec879fd 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,79 +42,68 @@
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
+ # TFリスナーの初期化
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
+ # ロボットモデルの取得（ジョイント目標値の設定に使用）
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',
- )
+ # プランニングの設定（動作プランナーと速度・加速度スケール）
+ 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
- # 動作速度の調整
- # Set 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_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.gripper_plan_request_params.max_velocity_scaling_factor = 1.0
- self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.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,
)
- # 可動範囲を制限する
- 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)
-
- self.arm.set_path_constraints(constraints)
-
- # 待機姿勢
- self._control_arm(0.0, 0.0, 0.3, 0, 0, 0.0)
+ # アームの可動範囲制限を設定してから待機姿勢に移動する
+ self.set_constraints()
+ self.control_arm(*self.STANDBY)
- # Call on_timer function every second
+ # 0.5秒ごとにon_timerを呼び出すタイマーを作成
self.timer = self.create_timer(0.5, self.on_timer)
def on_timer(self):
@@ -132,98 +121,81 @@ def on_timer(self):
STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3)
DISTANCE_THRESHOLD = 0.01
- # 経過時間と停止時間を計算(nsec)
- # 経過時間
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
+ 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,
- ]
- )
- # 把持対象の位置が停止していることを判定
- if tf_diff < DISTANCE_THRESHOLD:
- # 把持対象が3秒以上停止している場合ピッキング動作開始
- if TF_STOP_TIME > STOP_TIME_THRESHOLD:
- self._picking(tf_msg.transform.translation)
- else:
- 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)
-
- # 何かを掴んでいた時のためにハンドを開く
- 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)
+ if tf_elapsed_time > FILTERING_TIME:
+ return
- # 把持対象物に正対する
- self._control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg)
+ 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 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)
+ # 把持対象の位置が停止していることを判定
+ if tf_diff > DISTANCE_THRESHOLD:
+ self.tf_past = tf_msg
return
- # ハンドを閉じる
- self._control_gripper(GRIPPER_CLOSE)
+ # 把持対象が3秒以上停止している場合ピッキング動作開始
+ if tf_stop_time < STOP_TIME_THRESHOLD:
+ return
- # 移動する
- self._control_arm(0.12, 0.0, 0.17, 0, 90, 0)
+ self.picking(tf_msg.transform.translation)
- # 横へ移動する
- self._control_arm(0.0, -0.12, 0.17, 0, 90, -90)
+ def picking(self, target_position):
+ # 何かを掴んでいた時のためにハンドを開閉
+ self.move_gripper_angle(self.GRIPPER_OPEN)
- # 下ろす
- self._control_arm(0.0, -0.25, 0.05, 0, 90, -90)
+ x = target_position.x
+ y = target_position.y
+ theta_deg = math.degrees(math.atan2(y, x))
- # ハンドを開く
- self._control_gripper(GRIPPER_OPEN)
+ # ターゲットの正面に向ける
+ self.control_arm(0.0, 0.0, self.STANDBY[2], 0, 0, theta_deg)
- # 少しだけハンドを持ち上げる
- self._control_arm(0.0, -0.25, 0.10, 0, 90, -90)
+ # ピック動作（掴みに行く）
+ if not self.control_arm(x, y, self.GRASP_HEIGHT, 0, self.GRASP_PITCH, theta_deg):
+ # アーム動作に失敗した場合は待機姿勢に戻る
+ self.control_arm(*self.STANDBY)
+ return
- # 待機姿勢に戻る
- self._control_arm(0.0, 0.0, 0.3, 0, 0, 0)
+ self.move_gripper_angle(self.GRIPPER_CLOSE)
+
+ # プレース動作（移動して置く）
+ self.control_arm(*self.TRANSIT_1)
+ self.control_arm(*self.TRANSIT_2)
+ self.control_arm(*self.PLACE)
+ self.move_gripper_angle(self.GRIPPER_OPEN)
- # ハンドを閉じる
- self._control_gripper(GRIPPER_DEFAULT)
+ # 待機姿勢に戻る
+ self.control_arm(*self.PLACE_RETRACT)
+ self.control_arm(*self.STANDBY)
+ self.move_gripper_angle(self.GRIPPER_DEFAULT)
- # グリッパ制御
- def _control_gripper(self, angle):
+ 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 _control_arm(self, x, y, z, roll, pitch, yaw):
- # 位置姿勢の許容誤差
+ 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
- # 目標位置姿勢
target_pose = PoseStamped()
target_pose.header.frame_id = 'crane_plus_base'
target_pose.pose.position.x = x
@@ -236,11 +208,9 @@ def _control_arm(self, x, y, z, roll, pitch, yaw):
target_pose.pose.orientation.z = quat[2]
target_pose.pose.orientation.w = quat[3]
- # 目標位置姿勢の制約設定
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 +223,6 @@ 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,23 +237,49 @@ 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(
- self.crane_plus,
- self.arm,
- self.logger,
- single_plan_parameters=self.arm_plan_request_params,
+
+ return plan_and_execute(
+ self.crane_plus, self.arm, self.logger,
+ single_plan_parameters=self.arm_plan_params,
)
- return result
+
+ 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):
- # ros2の初期化
rclpy.init(args=args)
pick_and_place_tf_node = PickAndPlaceTf()
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()
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 cef4555..478bd77 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
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 e97753a..35a9e47 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
diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml
index 5b37710..5bc535f 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