From d7381cf7890ea62f5bd505d5cbc49d52894caddf Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Fri, 18 Oct 2024 17:00:28 +0900 Subject: [PATCH 1/4] =?UTF-8?q?=E7=82=B9=E7=BE=A4=E6=A4=9C=E5=87=BA?= =?UTF-8?q?=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E8=BF=BD=E5=8A=A0=20(#161)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 点群検出ノード追加 * pick_and_place_tf追加 * RealSenseのパラメータ名修正 * プレーシング位置調整 * 把持判定条件の整理 * 初期化動作修正 * 把持角度調整 * RVizの表示を調整 * RVizの表示を調整 * 使用するライブラリを整理 * スタイル修正 * Update README * 左右判別用enum作成、パラメータの定数化 * 点群が空のときの判定処理を修正 * 配信する点群を予めreserveする * スタイル修正 * 点群の取得範囲を修正 * 平面検出時の判定条件を修正 * コメント修正 --- sciurus17_examples/CMakeLists.txt | 6 + sciurus17_examples/README.md | 20 + .../launch/camera_example.launch.py | 89 +++++ sciurus17_examples/package.xml | 1 + sciurus17_examples/src/pick_and_place_tf.cpp | 347 ++++++++++++++++++ .../src/point_cloud_detection.cpp | 277 ++++++++++++++ sciurus17_moveit_config/config/moveit.rviz | 195 +++++++++- sciurus17_vision/launch/head_camera.launch.py | 4 +- 8 files changed, 930 insertions(+), 9 deletions(-) create mode 100644 sciurus17_examples/launch/camera_example.launch.py create mode 100644 sciurus17_examples/src/pick_and_place_tf.cpp create mode 100644 sciurus17_examples/src/point_cloud_detection.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 1da0016..d4ed8da 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(control_msgs REQUIRED) find_package(image_geometry REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(rclcpp_action REQUIRED) @@ -115,6 +117,8 @@ set(executable_list waist_control pick_and_place_right_arm_waist pick_and_place_left_arm + pick_and_place_tf + point_cloud_detection ) foreach(loop_var IN LISTS executable_list) add_executable(${loop_var} src/${loop_var}.cpp) @@ -126,6 +130,8 @@ foreach(loop_var IN LISTS executable_list) image_geometry moveit_ros_planning_interface OpenCV + pcl_ros + pcl_conversions rclcpp tf2_geometry_msgs control_msgs diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 58eb169..cec0bd2 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -19,6 +19,7 @@ - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) - [head\_camera\_tracking](#head_camera_tracking) - [chest\_camera\_tracking](#chest_camera_tracking) + - [point\_cloud\_detection](#point_cloud_detection) ## 準備（実機を使う場合） @@ -93,6 +94,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) - [head\_camera\_tracking](#head_camera_tracking) - [chest\_camera\_tracking](#chest_camera_tracking) +- [point\_cloud\_detection](#point_cloud_detection) 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 @@ -203,3 +205,21 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py [back to example list](#examples) --- + +### point_cloud_detection + +点群から物体を検出して掴むコード例です。 + +検出された物体位置はtfのフレームとして配信されます。 +tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。 +掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。 +物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。 + +次のコマンドを実行します +```sh +ros2 launch sciurus17_examples camera_example.launch.py example:='point_cloud_detection' +``` + +[back to example list](#examples) + +--- diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py new file mode 100644 index 0000000..29021d4 --- /dev/null +++ b/sciurus17_examples/launch/camera_example.launch.py @@ -0,0 +1,89 @@ +# Copyright 2024 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import SetParameter +from launch_ros.actions import Node +import yaml + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, 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 + return None + + +def generate_launch_description(): + description_loader = RobotDescriptionLoader() + + robot_description_semantic_config = load_file( + 'sciurus17_moveit_config', 'config/sciurus17.srdf') + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config} + + kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml') + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='point_cloud_detection', + description=('Set an example executable name: ' + '[point_cloud_detection]') + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description=('Set true when using the gazebo simulator.') + ) + + picking_node = Node(name="pick_and_place_tf", + package='sciurus17_examples', + executable='pick_and_place_tf', + output='screen', + parameters=[{'robot_description': description_loader.load()}, + robot_description_semantic, + kinematics_yaml]) + + detection_node = Node(name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples', + executable=LaunchConfiguration('example'), + output='screen') + + return LaunchDescription([ + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + picking_node, + declare_example_name, + detection_node + ]) diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index 50c7f53..92e4201 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -24,6 +24,7 @@ image_geometry libopencv-dev moveit_ros_planning_interface + pcl_ros rclcpp rclcpp_components tf2_geometry_msgs diff --git a/sciurus17_examples/src/pick_and_place_tf.cpp b/sciurus17_examples/src/pick_and_place_tf.cpp new file mode 100644 index 0000000..96d3fe9 --- /dev/null +++ b/sciurus17_examples/src/pick_and_place_tf.cpp @@ -0,0 +1,347 @@ +// Copyright 2024 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://github.com/ros-planning/moveit2_tutorials/blob +// /a547cf49ff7d1fe16a93dfe020c6027bcb035b51/doc/move_group_interface +// /src/move_group_interface_tutorial.cpp +// https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "moveit/move_group_interface/move_group_interface.h" +#include "pose_presets.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/convert.h" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +using namespace std::chrono_literals; +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +class PickAndPlaceTf : public rclcpp::Node +{ +public: + PickAndPlaceTf( + rclcpp::Node::SharedPtr move_group_neck_node, + rclcpp::Node::SharedPtr move_group_l_arm_node, + rclcpp::Node::SharedPtr move_group_l_gripper_node, + rclcpp::Node::SharedPtr move_group_r_arm_node, + rclcpp::Node::SharedPtr move_group_r_gripper_node) + : Node("pick_and_place_tf_node") + { + using namespace std::placeholders; + move_group_neck_ = + std::make_shared(move_group_neck_node, "neck_group"); + move_group_neck_->setMaxVelocityScalingFactor(0.1); + move_group_neck_->setMaxAccelerationScalingFactor(0.1); + + move_group_l_arm_ = + std::make_shared(move_group_l_arm_node, "l_arm_waist_group"); + move_group_l_arm_->setMaxVelocityScalingFactor(0.1); + move_group_l_arm_->setMaxAccelerationScalingFactor(0.1); + + move_group_l_gripper_ = + std::make_shared(move_group_l_gripper_node, "l_gripper_group"); + move_group_l_gripper_->setMaxVelocityScalingFactor(1.0); + move_group_l_gripper_->setMaxAccelerationScalingFactor(1.0); + + move_group_r_arm_ = + std::make_shared(move_group_r_arm_node, "r_arm_waist_group"); + move_group_r_arm_->setMaxVelocityScalingFactor(0.1); + move_group_r_arm_->setMaxAccelerationScalingFactor(0.1); + + move_group_r_gripper_ = + std::make_shared(move_group_r_gripper_node, "r_gripper_group"); + move_group_r_gripper_->setMaxVelocityScalingFactor(1.0); + move_group_r_gripper_->setMaxAccelerationScalingFactor(1.0); + + // 姿勢を初期化 + init_body(); + + // 可動範囲を制限する + moveit_msgs::msg::Constraints constraints; + constraints.name = "arm_constraints"; + + // 腰軸の可動範囲を制限する + moveit_msgs::msg::JointConstraint joint_constraint; + joint_constraint.joint_name = "waist_yaw_joint"; + joint_constraint.position = 0.0; + joint_constraint.tolerance_above = angles::from_degrees(45); + joint_constraint.tolerance_below = angles::from_degrees(45); + joint_constraint.weight = 1.0; + constraints.joint_constraints.push_back(joint_constraint); + + move_group_l_arm_->setPathConstraints(constraints); + move_group_r_arm_->setPathConstraints(constraints); + + tf_buffer_ = + std::make_unique(this->get_clock(), 2s); + tf_listener_ = + std::make_shared(*tf_buffer_); + + timer_ = this->create_wall_timer( + 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); + } + +private: + enum class ArmSide + { + LEFT, + RIGHT + }; + + void on_timer() + { + // target_0のtf位置姿勢を取得 + geometry_msgs::msg::TransformStamped tf_msg; + + try { + tf_msg = tf_buffer_->lookupTransform( + "base_link", "target_0", + tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform base_link to target: %s", + ex.what()); + return; + } + + rclcpp::Time now = this->get_clock()->now(); + constexpr std::chrono::nanoseconds FILTERING_TIME = 2s; + constexpr std::chrono::nanoseconds STOP_TIME_THRESHOLD = 3s; + constexpr double 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(); + constexpr double TARGET_Z_MIN_LIMIT = 0.04; + constexpr double TARGET_X_MIN_LIMIT = 0.13; + constexpr double TARGET_X_MAX_LIMIT = 0.3; + + // 掴む物体位置を制限する + if (tf.getOrigin().z() < TARGET_Z_MIN_LIMIT) { + return; + } + if (tf.getOrigin().x() < TARGET_X_MIN_LIMIT || tf.getOrigin().x() > TARGET_X_MAX_LIMIT) { + return; + } + + // 検出されてから2秒以上経過した物体は掴まない + if (TF_ELAPSED_TIME > FILTERING_TIME.count()) { + return; + } + + // 動いている物体は掴まない + double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length(); + if (tf_diff > DISTANCE_THRESHOLD) { + tf_past_ = tf; + return; + } + + // 物体が3秒以上停止している場合ピッキング動作開始 + if (TF_STOP_TIME < STOP_TIME_THRESHOLD.count()) { + return; + } + + picking(tf.getOrigin()); + } + + void init_body() + { + const double INITIAL_YAW_ANGLE = angles::from_degrees(0.0); + const double INITIAL_PITCH_ANGLE = angles::from_degrees(-80.0); + + move_group_l_arm_->setNamedTarget("l_arm_waist_init_pose"); + move_group_l_arm_->move(); + move_group_r_arm_->setNamedTarget("r_arm_waist_init_pose"); + move_group_r_arm_->move(); + + std::vector joint_values; + joint_values.push_back(INITIAL_YAW_ANGLE); + joint_values.push_back(INITIAL_PITCH_ANGLE); + move_group_neck_->setJointValueTarget(joint_values); + move_group_neck_->move(); + } + + void picking(tf2::Vector3 target_position) + { + // グリッパ開閉角度 + constexpr double GRIPPER_CLOSE = 0.0; + const double GRIPPER_OPEN = angles::from_degrees(50.0); + const double GRIPPER_GRASP = angles::from_degrees(20.0); + + // 物体を置く位置 + constexpr double PLACE_POSITION_X = 0.35; + constexpr double PLACE_POSITION_Y = 0.0; + constexpr double PLACE_POSITION_Z = 0.05; + + // 物体位置のオフセット + constexpr double APPROACH_OFFSET_Z = 0.12; + constexpr double GRASP_OFFSET_Z = 0.08; + + // 物体位置に応じて左右の腕を切り替え + ArmSide current_arm; + if (target_position.y() > 0) { + current_arm = ArmSide::LEFT; + } else { + current_arm = ArmSide::RIGHT; + } + + // 何かを掴んでいた時のためにハンドを開閉 + control_gripper(current_arm, GRIPPER_OPEN); + control_gripper(current_arm, GRIPPER_CLOSE); + + // 掴む準備をする + control_arm( + current_arm, + target_position.x(), target_position.y(), target_position.z() + APPROACH_OFFSET_Z); + + // ハンドを開く + control_gripper(current_arm, GRIPPER_OPEN); + + // 掴みに行く + control_arm( + current_arm, + target_position.x(), target_position.y(), target_position.z() + GRASP_OFFSET_Z); + + // ハンドを閉じる + control_gripper(current_arm, GRIPPER_GRASP); + + // 持ち上げる + control_arm( + current_arm, + target_position.x(), target_position.y(), target_position.z() + APPROACH_OFFSET_Z); + + // 移動する + control_arm( + current_arm, + PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z + APPROACH_OFFSET_Z); + + // 下ろす + control_arm( + current_arm, + PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z + GRASP_OFFSET_Z); + + // ハンドを開く + control_gripper(current_arm, GRIPPER_OPEN); + + // 少しだけハンドを持ち上げる + control_arm( + current_arm, + PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z + APPROACH_OFFSET_Z); + + // 初期姿勢に戻る + init_arm(current_arm); + + // ハンドを閉じる + control_gripper(current_arm, GRIPPER_CLOSE); + } + + // グリッパ制御 + void control_gripper(const ArmSide current_arm, const double angle) + { + auto joint_values = move_group_l_gripper_->getCurrentJointValues(); + + if (current_arm == ArmSide::LEFT) { + joint_values[0] = -angle; + move_group_l_gripper_->setJointValueTarget(joint_values); + move_group_l_gripper_->move(); + } + if (current_arm == ArmSide::RIGHT) { + joint_values[0] = angle; + move_group_r_gripper_->setJointValueTarget(joint_values); + move_group_r_gripper_->move(); + } + } + + // アーム制御 + void control_arm( + const ArmSide current_arm, const double x, const double y, const double z) + { + if (current_arm == ArmSide::LEFT) { + move_group_l_arm_->setPoseTarget( + pose_presets::left_arm_downward(x, y, z)); + move_group_l_arm_->move(); + } + if (current_arm == ArmSide::RIGHT) { + move_group_r_arm_->setPoseTarget( + pose_presets::right_arm_downward(x, y, z)); + move_group_r_arm_->move(); + } + } + + void init_arm(const ArmSide current_arm) + { + if (current_arm == ArmSide::LEFT) { + move_group_l_arm_->setNamedTarget("l_arm_waist_init_pose"); + move_group_l_arm_->move(); + } + if (current_arm == ArmSide::RIGHT) { + move_group_r_arm_->setNamedTarget("r_arm_waist_init_pose"); + move_group_r_arm_->move(); + } + } + + std::shared_ptr move_group_neck_; + std::shared_ptr move_group_l_arm_; + std::shared_ptr move_group_l_gripper_; + std::shared_ptr move_group_r_arm_; + std::shared_ptr move_group_r_gripper_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + tf2::Stamped tf_past_; +}; + +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_neck_node = + rclcpp::Node::make_shared("move_group_neck_node", node_options); + auto move_group_l_arm_node = + rclcpp::Node::make_shared("move_group_l_arm_node", node_options); + auto move_group_l_gripper_node = + rclcpp::Node::make_shared("move_group_l_gripper_node", node_options); + auto move_group_r_arm_node = + rclcpp::Node::make_shared("move_group_r_arm_node", node_options); + auto move_group_r_gripper_node = + rclcpp::Node::make_shared("move_group_r_gripper_node", node_options); + + rclcpp::executors::MultiThreadedExecutor exec; + auto pick_and_place_tf_node = std::make_shared( + move_group_neck_node, + move_group_l_arm_node, + move_group_l_gripper_node, + move_group_r_arm_node, + move_group_r_gripper_node); + exec.add_node(pick_and_place_tf_node); + exec.add_node(move_group_neck_node); + exec.add_node(move_group_l_arm_node); + exec.add_node(move_group_l_gripper_node); + exec.add_node(move_group_r_arm_node); + exec.add_node(move_group_r_gripper_node); + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_examples/src/point_cloud_detection.cpp b/sciurus17_examples/src/point_cloud_detection.cpp new file mode 100644 index 0000000..879d6d6 --- /dev/null +++ b/sciurus17_examples/src/point_cloud_detection.cpp @@ -0,0 +1,277 @@ +// Copyright 2024 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://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/passthrough.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/voxel_grid.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/planar_segmentation.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/extract_indices.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html +// + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "pcl/common/centroid.h" +#include "pcl/common/common.h" +#include "pcl/filters/extract_indices.h" +#include "pcl/filters/passthrough.h" +#include "pcl/filters/voxel_grid.h" +#include "pcl/io/pcd_io.h" +#include "pcl/kdtree/kdtree.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl/segmentation/extract_clusters.h" +#include "pcl/segmentation/sac_segmentation.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl_ros/transforms.hpp" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +class PointCloudSubscriber : public rclcpp::Node +{ +public: + PointCloudSubscriber() + : Node("point_cloud_detection") + { + point_cloud_subscription_ = this->create_subscription( + "/head_camera/depth/color/points", + 10, + std::bind(&PointCloudSubscriber::point_cloud_callback, this, std::placeholders::_1)); + + publisher_ = this->create_publisher("/classified_points", 10); + + tf_broadcaster_ = + std::make_unique(*this); + + tf_buffer_ = + std::make_unique(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + } + +private: + rclcpp::Subscription::SharedPtr point_cloud_subscription_; + rclcpp::Publisher::SharedPtr publisher_; + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + + void point_cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + // カメラ座標系における点群をロボット座標系に変換 + geometry_msgs::msg::TransformStamped tf_msg; + + try { + tf_msg = tf_buffer_->lookupTransform( + "base_link", msg->header.frame_id, + tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform base_link to camera_depth_optical_frame: %s", + ex.what()); + return; + } + + sensor_msgs::msg::PointCloud2 cloud_transformed; + pcl_ros::transformPointCloud("base_link", tf_msg, *msg, cloud_transformed); + + // ROSメッセージの点群フォーマットからPCLのフォーマットに変換 + auto cloud = std::make_shared>(); + pcl::fromROSMsg(cloud_transformed, *cloud); + + // 物体検出の前処理として点群の取得範囲の制限と間引きを行う + // フィルタリング後に点群がない場合は検出処理をスキップする + if (preprocessing(cloud) == false) { + return; + } + + // 平面除去 + // 平面検知ができない場合は検出処理をスキップする + // 物体がアームと別の高さの平面に置かれている場合など、 + // Z軸方向のフィルタリングで不要な点群が除去できない場合に使用してみてください + /* + if (plane_extraction(cloud) == false) { + return; + } + */ + + // KdTreeを用いて点群を物体ごとに分類(クラスタリング)する + auto cluster_indices = clustering(cloud); + + // クラスタごとに色分けし、クラスタ位置をtfで配信する + auto cloud_output = std::make_shared>(); + broadcast_cluster_position(cloud, cloud_output, cluster_indices, cloud_transformed.header); + + // クラスタリングした点群を配信する + sensor_msgs::msg::PointCloud2 sensor_msg; + cloud_output->header = cloud->header; + pcl::toROSMsg(*cloud_output, sensor_msg); + publisher_->publish(sensor_msg); + } + + bool preprocessing(std::shared_ptr> & cloud) + { + // X軸方向0.08~0.5m以外の点群を削除 + pcl::PassThrough pass; + pass.setInputCloud(cloud); + pass.setFilterFieldName("x"); + pass.setFilterLimits(0.08, 0.5); + pass.filter(*cloud); + + // Z軸方向0.03~0.2m以外の点群を削除 + // 物体が乗っている平面の点群を削除します + pass.setInputCloud(cloud); + pass.setFilterFieldName("z"); + pass.setFilterLimits(0.03, 0.2); + pass.filter(*cloud); + + // Voxel gridで点群を間引く(ダウンサンプリング) + pcl::VoxelGrid sor; + sor.setInputCloud(cloud); + sor.setLeafSize(0.01f, 0.01f, 0.01f); + sor.filter(*cloud); + + // フィルタリング後に点群がない場合はfalseを返す + if (cloud->empty()) { + RCLCPP_INFO(this->get_logger(), "No point cloud in the detection area."); + return false; + } else { + return true; + } + } + + bool plane_extraction(std::shared_ptr> & cloud) + { + auto coefficients = std::make_shared(); + auto inliers = std::make_shared(); + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(0.01); + seg.setInputCloud(cloud); + seg.segment(*inliers, *coefficients); + + // 平面が検出できなかった場合 + if (inliers->indices.empty()) { + RCLCPP_INFO(this->get_logger(), "Could not estimate a planar model for the given dataset."); + return false; + } + + // 検出した平面を削除 + pcl::ExtractIndices extract; + extract.setInputCloud(cloud); + extract.setIndices(inliers); + extract.setNegative(true); + extract.filter(*cloud); + return true; + } + + std::vector clustering( + std::shared_ptr> & cloud) + { + auto tree = std::make_shared>(); + tree->setInputCloud(cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.02); + ec.setMinClusterSize(10); + ec.setMaxClusterSize(250); + ec.setSearchMethod(tree); + ec.setInputCloud(cloud); + ec.extract(cluster_indices); + return cluster_indices; + } + + void broadcast_cluster_position( + std::shared_ptr> & cloud_input, + std::shared_ptr> & cloud_output, + std::vector & cluster_indices, + std_msgs::msg::Header & tf_header) + { + int cluster_i = 0; + enum COLOR_RGB + { + RED = 0, + GREEN, + BLUE, + COLOR_MAX + }; + constexpr int CLUSTER_MAX = 10; + constexpr int CLUSTER_COLOR[CLUSTER_MAX][COLOR_MAX] = { + {230, 0, 18}, {243, 152, 18}, {255, 251, 0}, + {143, 195, 31}, {0, 153, 68}, {0, 158, 150}, + {0, 160, 233}, {0, 104, 183}, {29, 32, 136}, + {146, 7, 131} + }; + + for (const auto & point_indices : cluster_indices) { + auto cloud_cluster = std::make_shared>(); + cloud_cluster->points.reserve(cloud_input->points.size()); + // 点群の色を変更 + for (const auto & point_i : point_indices.indices) { + cloud_input->points[point_i].r = CLUSTER_COLOR[cluster_i][RED]; + cloud_input->points[point_i].g = CLUSTER_COLOR[cluster_i][GREEN]; + cloud_input->points[point_i].b = CLUSTER_COLOR[cluster_i][BLUE]; + cloud_cluster->points.push_back(cloud_input->points[point_i]); + } + // 点群数の入力 + cloud_cluster->width = cloud_cluster->points.size(); + cloud_cluster->height = 1; + // 無効なpointがないのでis_denseはtrue + cloud_cluster->is_dense = true; + // 配信用の点群に追加 + *cloud_output += *cloud_cluster; + + // tfの配信 + // 点群の重心位置を物体位置として配信する + Eigen::Vector4f cluster_centroid; + pcl::compute3DCentroid(*cloud_cluster, cluster_centroid); + geometry_msgs::msg::TransformStamped t; + t.header = tf_header; + t.child_frame_id = "target_" + std::to_string(cluster_i); + t.transform.translation.x = cluster_centroid.x(); + t.transform.translation.y = cluster_centroid.y(); + t.transform.translation.z = cluster_centroid.z(); + t.transform.rotation.x = 0.0; + t.transform.rotation.y = 0.0; + t.transform.rotation.z = 0.0; + t.transform.rotation.w = 1.0; + tf_broadcaster_->sendTransform(t); + + // 設定した最大クラスタ数を超えたら処理を終える + cluster_i++; + if (cluster_i >= CLUSTER_MAX) { + break; + } + } + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_moveit_config/config/moveit.rviz b/sciurus17_moveit_config/config/moveit.rviz index 7e52a93..daa9d41 100644 --- a/sciurus17_moveit_config/config/moveit.rviz +++ b/sciurus17_moveit_config/config/moveit.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 148 + Tree Height: 242 - Class: rviz_common/Help Name: Help - Class: rviz_common/Views @@ -444,6 +444,185 @@ Visualization Manager: Reliability Policy: Reliable Value: /chest_camera/image_raw Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /head_camera/color/image_raw + Value: true + Visibility: + ClassifiedPointCloud: true + Grid: false + Image: false + MotionPlanning: true + PointCloud2: false + TF: true + Value: true + Zoom Factor: 1 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ClassifiedPointCloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /classified_points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 1 + Frames: + All Enabled: false + base_link: + Value: true + body_link: + Value: false + chest_camera_link: + Value: false + dummy_mimic_fix_l: + Value: false + dummy_mimic_fix_r: + Value: false + head_camera_color_frame: + Value: false + head_camera_color_optical_frame: + Value: false + head_camera_depth_frame: + Value: false + head_camera_depth_optical_frame: + Value: false + head_camera_link: + Value: false + l_gripperA_link: + Value: false + l_gripperB_link: + Value: false + l_link1: + Value: false + l_link2: + Value: false + l_link3: + Value: false + l_link4: + Value: false + l_link5: + Value: false + l_link5_armarker: + Value: false + l_link6: + Value: false + l_link7: + Value: false + neck_pitch_link: + Value: false + neck_yaw_link: + Value: false + r_gripperA_link: + Value: false + r_gripperB_link: + Value: false + r_link1: + Value: false + r_link2: + Value: false + r_link3: + Value: false + r_link4: + Value: false + r_link5: + Value: false + r_link5_armarker: + Value: false + r_link6: + Value: false + r_link7: + Value: false + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + base_link: + body_link: + chest_camera_link: + {} + l_link1: + l_link2: + l_link3: + l_link4: + l_link5: + l_link5_armarker: + {} + l_link6: + l_link7: + l_gripperA_link: + {} + l_gripperB_link: + {} + neck_yaw_link: + neck_pitch_link: + head_camera_link: + head_camera_color_frame: + head_camera_color_optical_frame: + {} + head_camera_depth_frame: + head_camera_depth_optical_frame: + {} + r_link1: + r_link2: + r_link3: + r_link4: + r_link5: + r_link5_armarker: + {} + r_link6: + r_link7: + r_gripperA_link: + {} + r_gripperB_link: + {} + dummy_mimic_fix_l: + {} + dummy_mimic_fix_r: + {} + Update Interval: 0 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -483,22 +662,24 @@ Visualization Manager: Yaw: 0 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false Height: 975 Help: collapsed: false Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Image: collapsed: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd00000002000000000000024200000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d000000d1000000c900fffffffb0000000a0049006d0061006700650100000114000000d40000002800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ee000001c40000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00ffffff00000001000001c500000375fc0200000001fb0000000a00560069006500770073000000003d00000375000000a400ffffff000002680000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000200000000000001f300000375fc0200000004fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d0000012f000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000172000002400000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00ffffff000000010000016600000375fc0200000003fb0000000c00430061006d006500720061010000003d000001a80000002800fffffffb0000000a0049006d00610067006501000001eb000001c70000002800fffffffb0000000a00560069006500770073000000003d00000375000000a400ffffff000002900000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1200 - X: 156 - Y: 29 + collapsed: false + Width: 1525 + X: 171 + Y: 38 diff --git a/sciurus17_vision/launch/head_camera.launch.py b/sciurus17_vision/launch/head_camera.launch.py index f70cd87..a7cc0c8 100644 --- a/sciurus17_vision/launch/head_camera.launch.py +++ b/sciurus17_vision/launch/head_camera.launch.py @@ -27,8 +27,8 @@ def generate_launch_description(): 'camera_namespace': '', 'camera_name': 'head_camera', 'device_type': 'd415', - 'rgb_camera.profile': '640x360x30', - 'depth_module.profile': '640x360x30', + 'rgb_camera.color_profile': '640x360x30', + 'depth_module.depth_profile': '640x360x30', 'pointcloud.enable': 'true', 'align_depth.enable': 'true', }.items() From 15800b2ab8472f181d678f05cbf5a48944d652ae Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Tue, 5 Nov 2024 17:28:20 +0900 Subject: [PATCH 2/4] 3.1.0 (#162) --- sciurus17/package.xml | 2 +- sciurus17_control/package.xml | 2 +- sciurus17_examples/package.xml | 2 +- sciurus17_gazebo/package.xml | 2 +- sciurus17_moveit_config/package.xml | 2 +- sciurus17_tools/package.xml | 2 +- sciurus17_vision/package.xml | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/sciurus17/package.xml b/sciurus17/package.xml index c415aaa..6b66679 100644 --- a/sciurus17/package.xml +++ b/sciurus17/package.xml @@ -2,7 +2,7 @@ sciurus17 - 3.0.0 + 3.1.0 ROS 2 package suite of Sciurus17 RT Corporation diff --git a/sciurus17_control/package.xml b/sciurus17_control/package.xml index dbc8310..c23d319 100644 --- a/sciurus17_control/package.xml +++ b/sciurus17_control/package.xml @@ -2,7 +2,7 @@ sciurus17_control - 3.0.0 + 3.1.0 The Sciurus17 control package RT Corporation Apache License 2.0 diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index 92e4201..5c49b0c 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -2,7 +2,7 @@ sciurus17_examples - 3.0.0 + 3.1.0 examples of Sciurus17 ROS package RT Corporation diff --git a/sciurus17_gazebo/package.xml b/sciurus17_gazebo/package.xml index 3828c96..c957cb6 100644 --- a/sciurus17_gazebo/package.xml +++ b/sciurus17_gazebo/package.xml @@ -2,7 +2,7 @@ sciurus17_gazebo - 3.0.0 + 3.1.0 The sciurus17_gazebo package RT Corporation Apache License 2.0 diff --git a/sciurus17_moveit_config/package.xml b/sciurus17_moveit_config/package.xml index a90dff5..e039840 100644 --- a/sciurus17_moveit_config/package.xml +++ b/sciurus17_moveit_config/package.xml @@ -2,7 +2,7 @@ sciurus17_moveit_config - 3.0.0 + 3.1.0 An automatically generated package with all the configuration and launch files for using the sciurus17 with the MoveIt Motion Planning Framework diff --git a/sciurus17_tools/package.xml b/sciurus17_tools/package.xml index ab010ac..31773a6 100644 --- a/sciurus17_tools/package.xml +++ b/sciurus17_tools/package.xml @@ -2,7 +2,7 @@ sciurus17_tools - 3.0.0 + 3.1.0 The sciurus17_tools package RT Corporation diff --git a/sciurus17_vision/package.xml b/sciurus17_vision/package.xml index 5e74b66..f1e41bf 100644 --- a/sciurus17_vision/package.xml +++ b/sciurus17_vision/package.xml @@ -2,7 +2,7 @@ sciurus17_vision - 3.0.0 + 3.1.0 The sciurus17_vision package RT Corporation Apache License 2.0 From e95d8419aeddf768fd69a32d466691877c9b1811 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 21 Nov 2024 19:27:45 +0900 Subject: [PATCH 3/4] =?UTF-8?q?ROS=202=20Jazzy=E3=81=AB=E5=AF=BE=E5=BF=9C?= =?UTF-8?q?=20(#158)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * jazzyでビルドが通るように修正 * capabilitiesに対応 * gz-sim対応 * OMPLの設定ファイルを追加 * doc, ciをJazzy対応 * Revert "jazzyでビルドが通るように修正" This reverts commit 7fdbb478f5dcc44fbe624fb11c98c4609347d08c. * 誤ってフォーマッターをかけてしまっていたので修正します * usb camの依存パッケージのバージョンについて記載 * リリースされるまでバージョン指定を入れておく * two handsをompl設定に追加 * MoveitConfigsBuilder周辺の記載を変更 * Update README.md Co-authored-by: Kuwamai * Update sciurus17_moveit_config/launch/run_move_group.launch.py Co-authored-by: Kuwamai * Update README.md Co-authored-by: Kuwamai * Update README.md Co-authored-by: Kuwamai * READMEに点群検出サンプルの依存関係に追記 * add dependency in ci * ci通るように修正 --------- Co-authored-by: Kuwamai --- .github/workflows/industrial_ci.yml | 4 +- README.en.md | 7 +- README.md | 9 +- .../launch/sciurus17_control.launch.py | 2 +- .../launch/camera_example.launch.py | 6 +- sciurus17_examples/launch/demo.launch.py | 2 +- sciurus17_examples/launch/example.launch.py | 4 +- sciurus17_examples/src/color_detection.cpp | 2 +- sciurus17_gazebo/gui/gui.config | 72 ++--- .../launch/sciurus17_with_table.launch.py | 18 +- sciurus17_gazebo/worlds/table.sdf | 12 +- .../config/ompl_planning.yaml | 285 ++++++++++++++++++ .../launch/run_move_group.launch.py | 19 +- .../launch/setup_assistant.launch.py | 3 +- 14 files changed, 372 insertions(+), 73 deletions(-) create mode 100644 sciurus17_moveit_config/config/ompl_planning.yaml diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index e6ef27d..8993dd3 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -11,13 +11,13 @@ on: env: UPSTREAM_WORKSPACE: .ci.rosinstall - + BEFORE_INSTALL_TARGET_DEPENDENCIES: "sudo apt-get update -qq && sudo apt-get -qq install -y libpcl-dev" jobs: industrial_ci: strategy: matrix: env: - - { ROS_DISTRO: humble, ROS_REPO: ros, BEFORE_RUN_TARGET_TEST_EMBED: "ici_with_unset_variables source /root/target_ws/install/setup.bash" } + - { ROS_DISTRO: jazzy, ROS_REPO: ros, BEFORE_RUN_TARGET_TEST_EMBED: "ici_with_unset_variables source /root/target_ws/install/setup.bash" } runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 diff --git a/README.en.md b/README.en.md index e2f3758..2cf6fab 100644 --- a/README.en.md +++ b/README.en.md @@ -25,6 +25,7 @@ ROS 2 package suite of Sciurus17. ## Supported ROS 2 distributions - Humble +- Jazzy ### ROS 1 @@ -37,9 +38,9 @@ ROS 2 package suite of Sciurus17. - [Product page](https://www.rt-net.jp/products/sciurus17) - [Web Shop](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3895&language=en) - Linux OS - - Ubuntu 22.04 + - Ubuntu 24.04 - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/Installation.html) ## Installation @@ -47,7 +48,7 @@ ROS 2 package suite of Sciurus17. ```sh # Setup ROS environment -source /opt/ros/humble/setup.bash +source /opt/ros/jazzy/setup.bash # Download packages mkdir -p ~/ros2_ws/src diff --git a/README.md b/README.md index 2e826f8..8fadce3 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ ROS 2 package suite of Sciurus17. ## Supported ROS 2 distributions - Humble +- Jazzy ### ROS 1 @@ -38,9 +39,9 @@ ROS 2 package suite of Sciurus17. - [製品ページ](https://www.rt-net.jp/products/sciurus17) - [ウェブショップ](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3895) - Linux OS - - Ubuntu 22.04 + - Ubuntu 24.04 - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) + - [Jazzy Jalisco](https://docs.ros.org/en/jazzy/Installation.html) ## Installation @@ -48,16 +49,18 @@ ROS 2 package suite of Sciurus17. ```sh # Setup ROS environment -source /opt/ros/humble/setup.bash +source /opt/ros/jazzy/setup.bash # Download packages mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src +git clone -b ros2 https://github.com/rt-net/rt_manipulators_cpp.git git clone -b ros2 https://github.com/rt-net/sciurus17_ros.git git clone -b ros2 https://github.com/rt-net/sciurus17_description.git # Install dependencies rosdep install -r -y -i --from-paths . +sudo apt install libpcl-dev # Build & Install cd ~/ros2_ws diff --git a/sciurus17_control/launch/sciurus17_control.launch.py b/sciurus17_control/launch/sciurus17_control.launch.py index fb782a9..514912f 100644 --- a/sciurus17_control/launch/sciurus17_control.launch.py +++ b/sciurus17_control/launch/sciurus17_control.launch.py @@ -15,12 +15,12 @@ import os from ament_index_python.packages import get_package_share_directory -from sciurus17_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import ExecuteProcess from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from sciurus17_description.robot_description_loader import RobotDescriptionLoader def generate_launch_description(): diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py index 29021d4..02820a2 100644 --- a/sciurus17_examples/launch/camera_example.launch.py +++ b/sciurus17_examples/launch/camera_example.launch.py @@ -15,12 +15,12 @@ import os from ament_index_python.packages import get_package_share_directory -from sciurus17_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import SetParameter from launch_ros.actions import Node +from launch_ros.actions import SetParameter +from sciurus17_description.robot_description_loader import RobotDescriptionLoader import yaml @@ -67,7 +67,7 @@ def generate_launch_description(): description=('Set true when using the gazebo simulator.') ) - picking_node = Node(name="pick_and_place_tf", + picking_node = Node(name='pick_and_place_tf', package='sciurus17_examples', executable='pick_and_place_tf', output='screen', diff --git a/sciurus17_examples/launch/demo.launch.py b/sciurus17_examples/launch/demo.launch.py index ee446ef..b81cb8b 100644 --- a/sciurus17_examples/launch/demo.launch.py +++ b/sciurus17_examples/launch/demo.launch.py @@ -15,13 +15,13 @@ import os from ament_index_python.packages import get_package_share_directory -from sciurus17_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +from sciurus17_description.robot_description_loader import RobotDescriptionLoader def generate_launch_description(): diff --git a/sciurus17_examples/launch/example.launch.py b/sciurus17_examples/launch/example.launch.py index 959ddc6..b2fd120 100644 --- a/sciurus17_examples/launch/example.launch.py +++ b/sciurus17_examples/launch/example.launch.py @@ -15,12 +15,12 @@ import os from ament_index_python.packages import get_package_share_directory -from sciurus17_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch_ros.actions import SetParameter from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.actions import SetParameter +from sciurus17_description.robot_description_loader import RobotDescriptionLoader import yaml diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index cf93027..71da591 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -22,7 +22,7 @@ #include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" using std::placeholders::_1; namespace sciurus17_examples diff --git a/sciurus17_gazebo/gui/gui.config b/sciurus17_gazebo/gui/gui.config index 72437fb..574721b 100644 --- a/sciurus17_gazebo/gui/gui.config +++ b/sciurus17_gazebo/gui/gui.config @@ -28,11 +28,11 @@ - + 3D View false docked - + ogre2 scene @@ -44,82 +44,82 @@ - + floating 5 5 false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + World control false false @@ -132,7 +132,7 @@ - + true true @@ -143,7 +143,7 @@ - + World stats false false @@ -156,7 +156,7 @@ - + true true @@ -166,7 +166,7 @@ - + false 0 0 @@ -175,12 +175,12 @@ floating false #666666 - + - + false 250 0 @@ -189,12 +189,12 @@ floating false #666666 - + - + false 0 50 @@ -203,7 +203,7 @@ floating false #777777 - + false @@ -211,7 +211,7 @@ - + false 250 50 @@ -220,12 +220,12 @@ floating false #777777 - + - + false 300 50 @@ -234,21 +234,21 @@ floating false #777777 - + - + false docked - + - + false docked - + diff --git a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py index c81988c..ad1a556 100644 --- a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py +++ b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py @@ -15,15 +15,15 @@ import os from ament_index_python.packages import get_package_share_directory -from sciurus17_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import ExecuteProcess from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.actions import SetParameter -from launch.substitutions import LaunchConfiguration +from sciurus17_description.robot_description_loader import RobotDescriptionLoader def generate_launch_description(): @@ -40,22 +40,22 @@ def generate_launch_description(): ) # PATHを追加で通さないとSTLファイルが読み込まれない - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], - 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], + 'GZ_SIM_RESOURCE_PATH': os.path.dirname( get_package_share_directory('sciurus17_description'))} world_file = os.path.join( get_package_share_directory('sciurus17_gazebo'), 'worlds', 'table.sdf') gui_config = os.path.join( get_package_share_directory('sciurus17_gazebo'), 'gui', 'gui.config') # -r オプションで起動時にシミュレーションをスタートしないと、コントローラが起動しない - ign_gazebo = ExecuteProcess( - cmd=['ign gazebo -r', world_file, '--gui-config', gui_config], + gz_sim = ExecuteProcess( + cmd=['gz sim -r', world_file, '--gui-config', gui_config], output='screen', additional_env=env, shell=True ) - ignition_spawn_entity = Node( + gz_sim_spawn_entity = Node( package='ros_gz_sim', executable='create', output='screen', @@ -136,8 +136,8 @@ def generate_launch_description(): SetParameter(name='use_sim_time', value=True), declare_use_head_camera, declare_use_chest_camera, - ign_gazebo, - ignition_spawn_entity, + gz_sim, + gz_sim_spawn_entity, spawn_joint_state_broadcaster, spawn_right_arm_controller, spawn_right_gripper_controller, diff --git a/sciurus17_gazebo/worlds/table.sdf b/sciurus17_gazebo/worlds/table.sdf index 9572111..4a4ab9c 100644 --- a/sciurus17_gazebo/worlds/table.sdf +++ b/sciurus17_gazebo/worlds/table.sdf @@ -6,16 +6,16 @@ 1.0 + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-user-commands-system" + name="gz::sim::systems::UserCommands"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> diff --git a/sciurus17_moveit_config/config/ompl_planning.yaml b/sciurus17_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..f56aa98 --- /dev/null +++ b/sciurus17_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,285 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# To optionally use Ruckig for jerk-limited smoothing, add this line to the request adapters below +# default_planning_request_adapters/AddRuckigTrajectorySmoothing +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath +planner_configs: + APSConfigDefault: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 36 # Number of hybrid paths generated per iteration + num_planners: 8 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +l_arm_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +l_arm_waist_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +r_arm_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +r_arm_waist_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + +two_arm_group: + planner_configs: + - APSConfigDefault + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault + diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index 5612775..6480e32 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -1,13 +1,13 @@ +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from moveit_configs_utils import MoveItConfigsBuilder 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 moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch from sciurus17_description.robot_description_loader import RobotDescriptionLoader -from ament_index_python.packages import get_package_share_directory def generate_launch_description(): @@ -20,7 +20,8 @@ def generate_launch_description(): 'loaded_description', default_value=description_loader.load(), description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in sciurus17_description.' + It is recommended to use RobotDescriptionLoader() \ + in sciurus17_description.' ) ) @@ -34,8 +35,12 @@ def generate_launch_description(): ) moveit_config = ( - MoveItConfigsBuilder("sciurus17") - .planning_pipelines("ompl", ["ompl"]) + MoveItConfigsBuilder('sciurus17') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .planning_pipelines(pipelines=['ompl']) .to_moveit_configs() ) @@ -43,6 +48,10 @@ def generate_launch_description(): 'robot_description': LaunchConfiguration('loaded_description') } + moveit_config.move_group_capabilities = { + 'capabilities': '' + } + # Move group ld.add_entity(generate_move_group_launch(moveit_config)) diff --git a/sciurus17_moveit_config/launch/setup_assistant.launch.py b/sciurus17_moveit_config/launch/setup_assistant.launch.py index ccf56d0..55c82e2 100644 --- a/sciurus17_moveit_config/launch/setup_assistant.launch.py +++ b/sciurus17_moveit_config/launch/setup_assistant.launch.py @@ -3,5 +3,6 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("sciurus17", package_name="sciurus17_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + 'sciurus17', package_name='sciurus17_moveit_config').to_moveit_configs() return generate_setup_assistant_launch(moveit_config) From 40d079e115c7b9a1dbdab5f13935dde2b2c9b7b6 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 25 Nov 2024 16:14:55 +0900 Subject: [PATCH 4/4] =?UTF-8?q?Mock=20components=E5=AF=BE=E5=BF=9C=20(#159?= =?UTF-8?q?)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * jazzyでビルドが通るように修正 * capabilitiesに対応 * gz-sim対応 * OMPLの設定ファイルを追加 * doc, ciをJazzy対応 * Revert "jazzyでビルドが通るように修正" This reverts commit 7fdbb478f5dcc44fbe624fb11c98c4609347d08c. * 誤ってフォーマッターをかけてしまっていたので修正します * usb camの依存パッケージのバージョンについて記載 * リリースされるまでバージョン指定を入れておく * two handsをompl設定に追加 * MoveitConfigsBuilder周辺の記載を変更 * add mock components support * mergeミスを修正 * 差分を消すために改行を追加 * fix indent --- sciurus17_examples/README.md | 15 +++++++++++++++ sciurus17_examples/launch/demo.launch.py | 8 ++++++++ 2 files changed, 23 insertions(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index cec0bd2..e585da4 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -9,6 +9,8 @@ - [3. move\_groupとcontrollerを起動する](#3-move_groupとcontrollerを起動する) - [準備 (Gazeboを使う場合)](#準備-gazeboを使う場合) - [1. move\_groupとGazeboを起動する](#1-move_groupとgazeboを起動する) + - [準備（Mock Componentsを使う場合）](#準備mock-componentsを使う場合) + - [1. move\_groupとcontrollerを起動する](#1-move_groupとcontrollerを起動する) - [サンプルプログラムを実行する](#サンプルプログラムを実行する) - [Gazeboでサンプルプログラムを実行する場合](#gazeboでサンプルプログラムを実行する場合) - [Examples](#examples) @@ -64,6 +66,19 @@ ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py ros2 launch sciurus17_gazebo sciurus17_with_table.launch.py use_head_camera:=false use_chest_camera:=false ``` +## 準備（Mock Componentsを使う場合） + +### 1. move_groupとcontrollerを起動する + +次のコマンドでmove_group (`sciurus17_moveit_config`)と +controller (`sciurus17_control`)を起動します。 + +```sh +ros2 launch sciurus17_examples demo.launch.py use_mock_components:=true +``` + +Mock Componentsではカメラを使ったサンプルを実行することはできません。 + ## サンプルプログラムを実行する 準備ができたらサンプルプログラムを実行します。 diff --git a/sciurus17_examples/launch/demo.launch.py b/sciurus17_examples/launch/demo.launch.py index b81cb8b..59d1f90 100644 --- a/sciurus17_examples/launch/demo.launch.py +++ b/sciurus17_examples/launch/demo.launch.py @@ -55,11 +55,18 @@ def generate_launch_description(): 'manipulator_config.yaml' ) + declare_use_mock_components = DeclareLaunchArgument( + 'use_mock_components', + default_value='false', + description='Use mock_components or not.' + ) + description_loader = RobotDescriptionLoader() description_loader.port_name = LaunchConfiguration('port_name') description_loader.baudrate = LaunchConfiguration('baudrate') description_loader.timeout_seconds = '1.0' description_loader.manipulator_config_file_path = config_file_path + description_loader.use_mock_components = LaunchConfiguration('use_mock_components') description = description_loader.load() @@ -98,6 +105,7 @@ def generate_launch_description(): declare_baudrate, declare_use_head_camera, declare_use_chest_camera, + declare_use_mock_components, move_group, control_node, head_camera_node,