diff --git a/README.en.md b/README.en.md index 8666cd24..68e5b3e6 100644 --- a/README.en.md +++ b/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 diff --git a/README.md b/README.md index a7962e7b..8eb5fc2e 100644 --- a/README.md +++ b/README.md @@ -24,17 +24,6 @@ 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 @@ -42,7 +31,6 @@ ROS 2 package suite of CRANE-X7. - [Melodic](https://github.com/rt-net/crane_x7_ros/tree/master) - [Noetic](https://github.com/rt-net/crane_x7_ros/tree/master) ->>>>>>> ros2 ## Requirements diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md index b9bb4a77..7b669146 100644 --- a/crane_x7_examples/README.md +++ b/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のフレームとして配信されます。 diff --git a/crane_x7_examples/package.xml b/crane_x7_examples/package.xml index 64228b1f..f5157997 100644 --- a/crane_x7_examples/package.xml +++ b/crane_x7_examples/package.xml @@ -21,7 +21,6 @@ image_geometry libopencv-dev moveit_ros_planning_interface - libpcl-all pcl_ros rclcpp realsense2_camera diff --git a/crane_x7_examples/src/color_detection.cpp b/crane_x7_examples/src/color_detection.cpp index bd1e853c..7ca8fabe 100644 --- a/crane_x7_examples/src/color_detection.cpp +++ b/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 +#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.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( + "/camera/color/image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); + + depth_subscription_ = this->create_subscription( + "/camera/aligned_depth_to_color/image_raw", 10, + std::bind(&ImageSubscriber::depth_callback, this, _1)); + + camera_info_subscription_ = this->create_subscription( + "/camera/color/camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); + + image_thresholded_publisher_ = + this->create_publisher("image_thresholded", 10); + + tf_broadcaster_ = + std::make_unique(*this); + } + +private: + rclcpp::Subscription::SharedPtr image_subscription_; + rclcpp::Subscription::SharedPtr depth_subscription_; + rclcpp::Subscription::SharedPtr camera_info_subscription_; + rclcpp::Publisher::SharedPtr image_thresholded_publisher_; + sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; + sensor_msgs::msg::Image::SharedPtr depth_image_; + std::unique_ptr 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(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()); + rclcpp::shutdown(); + return 0; +} diff --git a/crane_x7_moveit_config/config/controllers.yaml b/crane_x7_moveit_config/config/controllers.yaml index 58c44026..e5c8c323 100644 --- a/crane_x7_moveit_config/config/controllers.yaml +++ b/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 \ No newline at end of file +crane_x7_gripper_controller: + action_ns: gripper_cmd + type: GripperCommand + default: true + joints: + - crane_x7_gripper_finger_a_joint \ No newline at end of file diff --git a/crane_x7_moveit_config/launch/run_move_group.launch.py b/crane_x7_moveit_config/launch/run_move_group.launch.py index 0edfca4c..001246af 100644 --- a/crane_x7_moveit_config/launch/run_move_group.launch.py +++ b/crane_x7_moveit_config/launch/run_move_group.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2022 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -15,16 +15,15 @@ import os from ament_index_python.packages import get_package_share_directory +from crane_x7_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg -from moveit_configs_utils.launches import generate_move_group_launch -from moveit_configs_utils.launches import generate_moveit_rviz_launch -from moveit_configs_utils.launches \ - import generate_static_virtual_joint_tfs_launch -from moveit_configs_utils.launches import generate_rsp_launch +from launch_ros.actions import Node +import yaml + +# Reference: https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ +# examples/move_group_interface/launch/move_group.launch.py def load_file(package_name, file_path): @@ -34,9 +33,7 @@ def load_file(package_name, file_path): try: with open(absolute_file_path, 'r') as file: return file.read() - except ( - EnvironmentError - ): # parent of IOError, OSError *and* WindowsError where available + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available return None @@ -47,81 +44,114 @@ def load_yaml(package_name, file_path): try: with open(absolute_file_path, 'r') as file: return yaml.safe_load(file) - except ( - EnvironmentError - ): # parent of IOError, OSError *and* WindowsError where available + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available return None def generate_launch_description(): - ld = LaunchDescription() + description_loader = RobotDescriptionLoader() declare_loaded_description = DeclareLaunchArgument( 'loaded_description', - default_value='', + default_value=description_loader.load(), description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in \ - crane_x7_description.', - ) - - ld.add_action(declare_loaded_description) - - ld.add_action(DeclareBooleanLaunchArg('debug', default_value=False)) - - ld.add_action( - DeclareLaunchArgument( - 'rviz_config', - default_value=get_package_share_directory('crane_x7_moveit_config') - + '/config/moveit.rviz', - description='Set the path to rviz configuration file.', - ) + It is recommended to use RobotDescriptionLoader() in crane_x7_description.' ) - moveit_config = ( - MoveItConfigsBuilder('crane_x7') - .planning_scene_monitor( - publish_robot_description=True, - publish_robot_description_semantic=True, - ) - .robot_description( - file_path=os.path.join( - get_package_share_directory('crane_x7_description'), - 'urdf', - 'crane_x7.urdf.xacro', - ), - mappings={}, - ) - .robot_description_semantic( - file_path='config/crane_x7.srdf', - mappings={'model': 'crane_x7'}, - ) - .joint_limits(file_path='config/joint_limits.yaml') - .trajectory_execution( - file_path='config/controllers.yaml', moveit_manage_controllers=True - ) - .planning_pipelines(pipelines=['ompl']) - .robot_description_kinematics(file_path='config/kinematics.yaml') - .to_moveit_configs() + declare_rviz_config_file = DeclareLaunchArgument( + 'rviz_config_file', + default_value=get_package_share_directory( + 'crane_x7_moveit_config') + '/launch/run_move_group.rviz', + description='Set the path to rviz configuration file.' ) - moveit_config.robot_description = { - 'robot_description': LaunchConfiguration('loaded_description') - } + robot_description = {'robot_description': LaunchConfiguration('loaded_description')} - moveit_config.move_group_capabilities = { - 'capabilities': '' - } + robot_description_semantic_config = load_file( + 'crane_x7_moveit_config', 'config/crane_x7.srdf') + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config} - # Move group - ld.add_entity(generate_move_group_launch(moveit_config)) + joint_limits_yaml = load_yaml( + "crane_x7_moveit_config", "config/joint_limits.yaml" + ) + robot_description_planning = { + "robot_description_planning": joint_limits_yaml} + + kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml') + + # Planning Functionality + ompl_planning_pipeline_config = {'move_group': { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization \ + default_planner_request_adapters/FixWorkspaceBounds \ + default_planner_request_adapters/FixStartStateBounds \ + default_planner_request_adapters/FixStartStateCollision \ + default_planner_request_adapters/FixStartStatePathConstraints', + 'start_state_max_bounds_error': 0.1}} + ompl_planning_yaml = load_yaml('crane_x7_moveit_config', 'config/ompl_planning.yaml') + ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + controllers_yaml = load_yaml('crane_x7_moveit_config', 'config/controllers.yaml') + moveit_controllers = { + 'moveit_simple_controller_manager': controllers_yaml, + 'moveit_controller_manager': + 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} + + trajectory_execution = {'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.1} + + planning_scene_monitor_parameters = {'publish_planning_scene': True, + 'publish_geometry_updates': True, + 'publish_state_updates': True, + 'publish_transforms_updates': True} + + # Start the actual move_group node/action server + run_move_group_node = Node(package='moveit_ros_move_group', + executable='move_group', + output='screen', + parameters=[robot_description, + robot_description_semantic, + robot_description_planning, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters]) # RViz - ld.add_entity(generate_moveit_rviz_launch(moveit_config)) + rviz_config_file = LaunchConfiguration('rviz_config_file') + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_config_file], + parameters=[robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml]) # Static TF - ld.add_entity(generate_static_virtual_joint_tfs_launch(moveit_config)) + static_tf = Node(package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'base_link']) # Publish TF - ld.add_entity(generate_rsp_launch(moveit_config)) - - return ld + robot_state_publisher = Node(package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[robot_description]) + + return LaunchDescription([declare_loaded_description, + declare_rviz_config_file, + run_move_group_node, + rviz_node, + static_tf, + robot_state_publisher, + ])