ros2ブランチを取り込む

README.en.md

@@ -23,17 +23,6 @@ ROS 2 package suite of CRANE-X7.
 
 ## Supported ROS 2 distributions
 
-<<<<<<< HEAD
-## Supported ROS distributions
-
-- Melodic
-- Noetic
-
-### ROS 2
-
-- [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel)
-- [Humble](https://github.com/rt-net/crane_x7_ros/tree/humble-devel)
-=======
 - [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel)
 - Humble
 
@@ -50,7 +39,6 @@ ROS 2 package suite of CRANE-X7.
   - Ubuntu 20.04
 - ROS
   - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html)
->>>>>>> ros2
 
 ## Installation
 

README.md

@@ -24,25 +24,13 @@ ROS 2 package suite of CRANE-X7.
 
 ## Supported ROS 2 distributions
 
-<<<<<<< HEAD
-## サポートするROSディストリビューション
-
-- Melodic
-- Noetic
-
-### ROS 2
-
-- [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel)
-- [Humble](https://github.com/rt-net/crane_x7_ros/tree/humble-devel)
-=======
 - [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel)
 - Humble
 
 ### ROS
 
 - [Melodic](https://github.com/rt-net/crane_x7_ros/tree/master)
 - [Noetic](https://github.com/rt-net/crane_x7_ros/tree/master)
->>>>>>> ros2
 
 ## Requirements
 

crane_x7_examples/README.md

@@ -40,11 +40,7 @@
 CRANE-X7本体をPCに接続します。
 接続方法は製品マニュアルを参照してください。
 
-<<<<<<< HEAD
-### 簡易シミュレータを使う場合
-=======
 **※CRANE-X7本体が接触しないように、十分なスペースを確保してください。**
->>>>>>> ros2
 
 ### 2. USB通信ポートの接続を確認する
 
@@ -64,23 +60,17 @@ controller (`crane_x7_control`)を起動します。
 ros2 launch crane_x7_examples demo.launch.py port_name:=/dev/ttyUSB0
 ```
 
-<<<<<<< HEAD
-物理演算やセンサを含めたシミュレーションは、
-後述の[「Gazeboを使う場合」](#gazeboを使う場合)の手順に従ってください。
-
-### 実機を使う場合
-
-実機で動作を確認する場合、
-制御信号ケーブルを接続した状態で次のコマンドを実行します。
-=======
 #### RealSense D435マウンタ搭載モデルを使用する場合
 [RealSense D435マウンタ](https://github.com/rt-net/crane_x7_Hardware/blob/master/3d_print_parts/v1.0/CRANE-X7_HandA_RealSenseD435マウンタ.stl)を搭載している場合は次のコマンドを実行します。RealSense D435が起動し、camera_linkがロボットモデルに追加されます。
->>>>>>> ros2
 
 ```sh
-ros2 launch crane_x7_examples demo.launch.py port_name:=/dev/ttyUSB0 use_d435:=true
+roslaunch crane_x7_bringup demo.launch fake_execution:=true
 ```
 
+<<<<<<< HEAD
+物理演算やセンサを含めたシミュレーションは、
+後述の[「Gazeboを使う場合」](#gazeboを使う場合)の手順に従ってください。
+
 ## 準備 (Gazeboを使う場合)
 
 ![crane_x7_gazebo](https://rt-net.github.io/images/crane-x7/crane_x7_gazebo_ros2.png)
@@ -302,6 +292,7 @@ ros2 launch crane_x7_examples camera_example.launch.py example:='color_detection
 
 ### point_cloud_detection
 
+点群から物体を検出して掴むコード例です。
 点群から物体を検出して掴むコード例です。
 
 検出された物体位置はtfのフレームとして配信されます。

crane_x7_examples/package.xml

@@ -21,7 +21,6 @@
   <depend>image_geometry</depend>
   <depend>libopencv-dev</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>libpcl-all</depend>
   <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>realsense2_camera</depend>

crane_x7_examples/src/color_detection.cpp

@@ -187,3 +187,193 @@ int main(int argc, char * argv[])
   rclcpp::shutdown();
   return 0;
 }
+
+// Copyright 2023 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.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Reference:
+// https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html
+
+#include <cmath>
+#include <iostream>
+#include <iomanip>
+#include <memory>
+
+#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.h"
+#include "tf2/LinearMath/Matrix3x3.h"
+#include "tf2_ros/transform_broadcaster.h"
+#include "opencv2/opencv.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+#include "cv_bridge/cv_bridge.h"
+#include "image_geometry/pinhole_camera_model.h"
+using std::placeholders::_1;
+
+class ImageSubscriber : public rclcpp::Node
+{
+public:
+  ImageSubscriber()
+  : Node("color_detection")
+  {
+    image_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
+      "/camera/color/image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1));
+
+    depth_subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
+      "/camera/aligned_depth_to_color/image_raw", 10,
+      std::bind(&ImageSubscriber::depth_callback, this, _1));
+
+    camera_info_subscription_ = this->create_subscription<sensor_msgs::msg::CameraInfo>(
+      "/camera/color/camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1));
+
+    image_thresholded_publisher_ =
+      this->create_publisher<sensor_msgs::msg::Image>("image_thresholded", 10);
+
+    tf_broadcaster_ =
+      std::make_unique<tf2_ros::TransformBroadcaster>(*this);
+  }
+
+private:
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_subscription_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr depth_subscription_;
+  rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_subscription_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_thresholded_publisher_;
+  sensor_msgs::msg::CameraInfo::SharedPtr camera_info_;
+  sensor_msgs::msg::Image::SharedPtr depth_image_;
+  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+
+  void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+  {
+    // カメラのパラメータを取得してから処理を行う
+    if (camera_info_ && depth_image_) {
+      // 青い物体を検出するようにHSVの範囲を設定
+      // 周囲の明るさ等の動作環境に合わせて調整
+      const int LOW_H = 100, HIGH_H = 125;
+      const int LOW_S = 100, HIGH_S = 255;
+      const int LOW_V = 30, 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_thresholded;
+
+      // 画像の二値化
+      cv::inRange(
+        cv_img->image,
+        cv::Scalar(LOW_H, LOW_S, LOW_V),
+        cv::Scalar(HIGH_H, HIGH_S, HIGH_V),
+        img_thresholded);
+
+      // ノイズ除去の処理
+      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);
+
+        // 把持対象物までの距離を取得
+        // 把持対象物の表面より少し奥を掴むように設定
+        const double DEPTH_OFFSET = 0.015;
+        const auto cv_depth = cv_bridge::toCvShare(depth_image_, depth_image_->encoding);
+        // カメラから把持対象物の表面までの距離
+        const auto front_distance = cv_depth->image.at<ushort>(point) / 1000.0;
+        const auto center_distance = front_distance + DEPTH_OFFSET;
+
+        // 距離を取得できないか遠すぎる場合は把持しない
+        const double DEPTH_MAX = 0.5;
+        const double DEPTH_MIN = 0.2;
+        if (center_distance < DEPTH_MIN || center_distance > DEPTH_MAX) {
+          RCLCPP_INFO_STREAM(this->get_logger(), "Failed to get depth at" << point << ".");
+          return;
+        }
+
+        // 把持対象物の位置を計算
+        cv::Point3d object_position(
+          ray.x * center_distance,
+          ray.y * center_distance,
+          ray.z * center_distance);
+
+        // 把持対象物の位置を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);
+      }
+    }
+  }
+
+  void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg)
+  {
+    camera_info_ = msg;
+  }
+
+  void depth_callback(const sensor_msgs::msg::Image::SharedPtr msg)
+  {
+    depth_image_ = msg;
+  }
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ImageSubscriber>());
+  rclcpp::shutdown();
+  return 0;
+}

crane_x7_moveit_config/config/controllers.yaml

@@ -1,27 +1,23 @@
-# MoveIt uses this configuration for controller management
-moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
+controller_names:
+  - crane_x7_arm_controller
+  - crane_x7_gripper_controller
 
-moveit_simple_controller_manager:
-  controller_names:
-    - crane_x7_arm_controller
-    - crane_x7_gripper_controller
+crane_x7_arm_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: true
+  joints:
+    - crane_x7_shoulder_fixed_part_pan_joint
+    - crane_x7_shoulder_revolute_part_tilt_joint
+    - crane_x7_upper_arm_revolute_part_twist_joint
+    - crane_x7_upper_arm_revolute_part_rotate_joint
+    - crane_x7_lower_arm_fixed_part_joint
+    - crane_x7_lower_arm_revolute_part_joint
+    - crane_x7_wrist_joint
 
-  crane_x7_arm_controller:
-    action_ns: follow_joint_trajectory
-    type: FollowJointTrajectory
-    default: true
-    joints:
-      - crane_x7_shoulder_fixed_part_pan_joint
-      - crane_x7_shoulder_revolute_part_tilt_joint
-      - crane_x7_upper_arm_revolute_part_twist_joint
-      - crane_x7_upper_arm_revolute_part_rotate_joint
-      - crane_x7_lower_arm_fixed_part_joint
-      - crane_x7_lower_arm_revolute_part_joint
-      - crane_x7_wrist_joint
-
-  crane_x7_gripper_controller:
-    action_ns: gripper_cmd
-    type: GripperCommand
-    default: true
-    joints:
-      - crane_x7_gripper_finger_a_joint
+crane_x7_gripper_controller:
+  action_ns: gripper_cmd
+  type: GripperCommand
+  default: true
+  joints:
+    - crane_x7_gripper_finger_a_joint