From 7fdbb478f5dcc44fbe624fb11c98c4609347d08c Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 8 Jul 2024 16:39:17 +0900 Subject: [PATCH 01/15] =?UTF-8?q?jazzy=E3=81=A7=E3=83=93=E3=83=AB=E3=83=89?= =?UTF-8?q?=E3=81=8C=E9=80=9A=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 219 +++++++++++---------- 1 file changed, 112 insertions(+), 107 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index cf93027..a824151 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -22,127 +22,132 @@ #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 { -ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) -: Node("color_detection", options) -{ - image_subscription_ = this->create_subscription( - "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); + ColorDetection::ColorDetection(const rclcpp::NodeOptions &options) + : Node("color_detection", options) + { + image_subscription_ = this->create_subscription( + "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); - image_annotated_publisher_ = - this->create_publisher("image_annotated", 10); + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); - object_point_publisher_ = - this->create_publisher("target_position", 10); -} + object_point_publisher_ = + this->create_publisher("target_position", 10); + } -void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) -{ - // オレンジ色の物体を検出するようにHSVの範囲を設定 - const int LOW_H = 5, HIGH_H = 20; - const int LOW_S = 120, HIGH_S = 255; - const int LOW_V = 120, HIGH_V = 255; - - // 画像全体の10%以上の大きさで映った物体を検出 - const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; - - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換 - cv::Mat img_hsv; - cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); - - // 画像の二値化 - cv::Mat img_thresholded; - cv::inRange( - img_hsv, - 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::Mat img_annotated; - cv_img->image.copyTo(img_annotated, img_thresholded); - - // 二値化した領域の輪郭を取得 - std::vector> contours; - cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); - - if (contours.size()) { - // 最も面積の大きい領域を取得 - std::vector object_moments; - int max_area_i = -1; - int i = 0; - for (const auto & contour : contours) { - object_moments.push_back(cv::moments(contour)); - if (object_moments[max_area_i].m00 < object_moments[i].m00) { - max_area_i = i; + void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + // オレンジ色の物体を検出するようにHSVの範囲を設定 + const int LOW_H = 5, HIGH_H = 20; + const int LOW_S = 120, HIGH_S = 255; + const int LOW_V = 120, HIGH_V = 255; + + // 画像全体の10%以上の大きさで映った物体を検出 + const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; + + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); + + // 画像の二値化 + cv::Mat img_thresholded; + cv::inRange( + img_hsv, + 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::Mat img_annotated; + cv_img->image.copyTo(img_annotated, img_thresholded); + + // 二値化した領域の輪郭を取得 + std::vector> contours; + cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); + + if (contours.size()) + { + // 最も面積の大きい領域を取得 + std::vector object_moments; + int max_area_i = -1; + int i = 0; + for (const auto &contour : contours) + { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) + { + max_area_i = i; + } + i++; } - i++; - } - if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; - object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 検出領域と検出位置を描画 - const cv::Scalar ANNOTATE_COLOR(256, 0, 256); - const int ANNOTATE_THICKNESS = 4; - const int ANNOTATE_RADIUS = 10; - cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); - cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); - - // 画像の中心を原点とした検出位置に変換 - cv::Point2d translated_object_point; - translated_object_point.x = object_point.x - msg->width / 2.0; - translated_object_point.y = object_point.y - msg->height / 2.0; - - // 検出位置を-1.0 ~ 1.0に正規化 - cv::Point2d normalized_object_point_; - if (msg->width != 0 && msg->height != 0) { - normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); - normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); + if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) + { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; + object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + const cv::Scalar ANNOTATE_COLOR(256, 0, 256); + const int ANNOTATE_THICKNESS = 4; + const int ANNOTATE_RADIUS = 10; + cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); + cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); + + // 画像の中心を原点とした検出位置に変換 + cv::Point2d translated_object_point; + translated_object_point.x = object_point.x - msg->width / 2.0; + translated_object_point.y = object_point.y - msg->height / 2.0; + + // 検出位置を-1.0 ~ 1.0に正規化 + cv::Point2d normalized_object_point_; + if (msg->width != 0 && msg->height != 0) + { + normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); + normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); + } + + // 検出位置を配信 + geometry_msgs::msg::PointStamped object_point_msg; + object_point_msg.header = msg->header; + object_point_msg.point.x = normalized_object_point_.x; + object_point_msg.point.y = normalized_object_point_.y; + object_point_publisher_->publish(object_point_msg); } - - // 検出位置を配信 - geometry_msgs::msg::PointStamped object_point_msg; - object_point_msg.header = msg->header; - object_point_msg.point.x = normalized_object_point_.x; - object_point_msg.point.y = normalized_object_point_.y; - object_point_publisher_->publish(object_point_msg); } - } - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_annotated_msg = - cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); - image_annotated_publisher_->publish(*img_annotated_msg); -} + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_annotated_msg = + cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); + image_annotated_publisher_->publish(*img_annotated_msg); + } -} // namespace sciurus17_examples +} // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) From 40749ac921cb44ca43ddca7ba4b3606647d28d5e Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 9 Jul 2024 11:53:10 +0900 Subject: [PATCH 02/15] =?UTF-8?q?capabilities=E3=81=AB=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_moveit_config/launch/run_move_group.launch.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index 5612775..f58eae6 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -43,6 +43,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)) From 1491067bad8cd7398b7bf9fe1f838ac9f081eb64 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Tue, 9 Jul 2024 11:53:57 +0900 Subject: [PATCH 03/15] =?UTF-8?q?gz-sim=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_gazebo/gui/gui.config | 72 +++++++++---------- .../launch/sciurus17_with_table.launch.py | 14 ++-- sciurus17_gazebo/worlds/table.sdf | 12 ++-- 3 files changed, 49 insertions(+), 49 deletions(-) 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..2fe2559 100644 --- a/sciurus17_gazebo/launch/sciurus17_with_table.launch.py +++ b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py @@ -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"> From 45d2ccbe56c7e59402fc243d4883c7b8c7d71a30 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 12 Jul 2024 09:37:35 +0900 Subject: [PATCH 04/15] =?UTF-8?q?OMPL=E3=81=AE=E8=A8=AD=E5=AE=9A=E3=83=95?= =?UTF-8?q?=E3=82=A1=E3=82=A4=E3=83=AB=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/ompl_planning.yaml | 256 ++++++++++++++++++ 1 file changed, 256 insertions(+) create mode 100644 sciurus17_moveit_config/config/ompl_planning.yaml diff --git a/sciurus17_moveit_config/config/ompl_planning.yaml b/sciurus17_moveit_config/config/ompl_planning.yaml new file mode 100644 index 0000000..c700fa1 --- /dev/null +++ b/sciurus17_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,256 @@ +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 From 537cc7d56e3cb71f0e36858252e7fc391d043de1 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 17 Jul 2024 18:02:18 +0900 Subject: [PATCH 05/15] =?UTF-8?q?doc,=20ci=E3=82=92Jazzy=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/industrial_ci.yml | 2 +- README.en.md | 7 ++++--- README.md | 7 ++++--- 3 files changed, 9 insertions(+), 7 deletions(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index e6ef27d..f951dcf 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -17,7 +17,7 @@ jobs: 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..e889b74 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,7 +49,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 From 3531db7c88c8374cdedfe823e063cdaf3bcf4a70 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 18 Jul 2024 09:37:34 +0900 Subject: [PATCH 06/15] =?UTF-8?q?Revert=20"jazzy=E3=81=A7=E3=83=93?= =?UTF-8?q?=E3=83=AB=E3=83=89=E3=81=8C=E9=80=9A=E3=82=8B=E3=82=88=E3=81=86?= =?UTF-8?q?=E3=81=AB=E4=BF=AE=E6=AD=A3"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 7fdbb478f5dcc44fbe624fb11c98c4609347d08c. --- sciurus17_examples/src/color_detection.cpp | 219 ++++++++++----------- 1 file changed, 107 insertions(+), 112 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index a824151..cf93027 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -22,132 +22,127 @@ #include "geometry_msgs/msg/point_stamped.hpp" #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" -#include "cv_bridge/cv_bridge.hpp" +#include "cv_bridge/cv_bridge.h" using std::placeholders::_1; namespace sciurus17_examples { - ColorDetection::ColorDetection(const rclcpp::NodeOptions &options) - : Node("color_detection", options) - { - image_subscription_ = this->create_subscription( - "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); +ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) +: Node("color_detection", options) +{ + image_subscription_ = this->create_subscription( + "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); - image_annotated_publisher_ = - this->create_publisher("image_annotated", 10); + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); - object_point_publisher_ = - this->create_publisher("target_position", 10); - } + object_point_publisher_ = + this->create_publisher("target_position", 10); +} - void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) - { - // オレンジ色の物体を検出するようにHSVの範囲を設定 - const int LOW_H = 5, HIGH_H = 20; - const int LOW_S = 120, HIGH_S = 255; - const int LOW_V = 120, HIGH_V = 255; - - // 画像全体の10%以上の大きさで映った物体を検出 - const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; - - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換 - cv::Mat img_hsv; - cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); - - // 画像の二値化 - cv::Mat img_thresholded; - cv::inRange( - img_hsv, - 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::Mat img_annotated; - cv_img->image.copyTo(img_annotated, img_thresholded); - - // 二値化した領域の輪郭を取得 - std::vector> contours; - cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); - - if (contours.size()) - { - // 最も面積の大きい領域を取得 - std::vector object_moments; - int max_area_i = -1; - int i = 0; - for (const auto &contour : contours) - { - object_moments.push_back(cv::moments(contour)); - if (object_moments[max_area_i].m00 < object_moments[i].m00) - { - max_area_i = i; - } - i++; +void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + // オレンジ色の物体を検出するようにHSVの範囲を設定 + const int LOW_H = 5, HIGH_H = 20; + const int LOW_S = 120, HIGH_S = 255; + const int LOW_V = 120, HIGH_V = 255; + + // 画像全体の10%以上の大きさで映った物体を検出 + const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; + + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); + + // 画像の二値化 + cv::Mat img_thresholded; + cv::inRange( + img_hsv, + 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::Mat img_annotated; + cv_img->image.copyTo(img_annotated, img_thresholded); + + // 二値化した領域の輪郭を取得 + std::vector> contours; + cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); + + if (contours.size()) { + // 最も面積の大きい領域を取得 + std::vector object_moments; + int max_area_i = -1; + int i = 0; + for (const auto & contour : contours) { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) { + max_area_i = i; } + i++; + } - if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) - { - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; - object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 検出領域と検出位置を描画 - const cv::Scalar ANNOTATE_COLOR(256, 0, 256); - const int ANNOTATE_THICKNESS = 4; - const int ANNOTATE_RADIUS = 10; - cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); - cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); - - // 画像の中心を原点とした検出位置に変換 - cv::Point2d translated_object_point; - translated_object_point.x = object_point.x - msg->width / 2.0; - translated_object_point.y = object_point.y - msg->height / 2.0; - - // 検出位置を-1.0 ~ 1.0に正規化 - cv::Point2d normalized_object_point_; - if (msg->width != 0 && msg->height != 0) - { - normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); - normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); - } - - // 検出位置を配信 - geometry_msgs::msg::PointStamped object_point_msg; - object_point_msg.header = msg->header; - object_point_msg.point.x = normalized_object_point_.x; - object_point_msg.point.y = normalized_object_point_.y; - object_point_publisher_->publish(object_point_msg); + if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; + object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + const cv::Scalar ANNOTATE_COLOR(256, 0, 256); + const int ANNOTATE_THICKNESS = 4; + const int ANNOTATE_RADIUS = 10; + cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); + cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); + + // 画像の中心を原点とした検出位置に変換 + cv::Point2d translated_object_point; + translated_object_point.x = object_point.x - msg->width / 2.0; + translated_object_point.y = object_point.y - msg->height / 2.0; + + // 検出位置を-1.0 ~ 1.0に正規化 + cv::Point2d normalized_object_point_; + if (msg->width != 0 && msg->height != 0) { + normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); + normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); } - } - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_annotated_msg = - cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); - image_annotated_publisher_->publish(*img_annotated_msg); + // 検出位置を配信 + geometry_msgs::msg::PointStamped object_point_msg; + object_point_msg.header = msg->header; + object_point_msg.point.x = normalized_object_point_.x; + object_point_msg.point.y = normalized_object_point_.y; + object_point_publisher_->publish(object_point_msg); + } } -} // namespace sciurus17_examples + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_annotated_msg = + cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); + image_annotated_publisher_->publish(*img_annotated_msg); +} + +} // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) From 43216c286b3c3be2e5cd04c177fd486e32e2ad93 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 18 Jul 2024 09:38:06 +0900 Subject: [PATCH 07/15] =?UTF-8?q?=E8=AA=A4=E3=81=A3=E3=81=A6=E3=83=95?= =?UTF-8?q?=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=82=BF=E3=83=BC=E3=82=92?= =?UTF-8?q?=E3=81=8B=E3=81=91=E3=81=A6=E3=81=97=E3=81=BE=E3=81=A3=E3=81=A6?= =?UTF-8?q?=E3=81=84=E3=81=9F=E3=81=AE=E3=81=A7=E4=BF=AE=E6=AD=A3=E3=81=97?= =?UTF-8?q?=E3=81=BE=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 3e9401e762b9099426de65cdfed0236871cab41b Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 18 Jul 2024 10:05:34 +0900 Subject: [PATCH 08/15] =?UTF-8?q?usb=20cam=E3=81=AE=E4=BE=9D=E5=AD=98?= =?UTF-8?q?=E3=83=91=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E3=81=AE=E3=83=90?= =?UTF-8?q?=E3=83=BC=E3=82=B8=E3=83=A7=E3=83=B3=E3=81=AB=E3=81=A4=E3=81=84?= =?UTF-8?q?=E3=81=A6=E8=A8=98=E8=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/README.md b/README.md index e889b74..b6ac8de 100644 --- a/README.md +++ b/README.md @@ -46,6 +46,9 @@ ROS 2 package suite of Sciurus17. ## Installation ### Build from source +2024 July 18現在、カメラを使ったサンプルで使用しているusb_camを正常に実行するためには +aptに登録されているものよりも最新のimage_commonのバージョンを使用する必要があります +そのためsciurus17関連のパッケージと合わせてソースからビルドします ```sh # Setup ROS environment @@ -56,6 +59,8 @@ mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src git clone -b ros2 https://github.com/rt-net/sciurus17_ros.git git clone -b ros2 https://github.com/rt-net/sciurus17_description.git +# To run examples with camera +git clone -b 5.1.4 https://github.com/ros-perception/image_common.git # Install dependencies rosdep install -r -y -i --from-paths . From 5f62ad73c981e88512a63888ad73cf68cd432953 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 19 Jul 2024 14:03:53 +0900 Subject: [PATCH 09/15] =?UTF-8?q?=E3=83=AA=E3=83=AA=E3=83=BC=E3=82=B9?= =?UTF-8?q?=E3=81=95=E3=82=8C=E3=82=8B=E3=81=BE=E3=81=A7=E3=83=90=E3=83=BC?= =?UTF-8?q?=E3=82=B8=E3=83=A7=E3=83=B3=E6=8C=87=E5=AE=9A=E3=82=92=E5=85=A5?= =?UTF-8?q?=E3=82=8C=E3=81=A6=E3=81=8A=E3=81=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index b6ac8de..3c29212 100644 --- a/README.md +++ b/README.md @@ -57,6 +57,7 @@ source /opt/ros/jazzy/setup.bash # Download packages mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src +git clone -b feature/support-jazzy 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 # To run examples with camera From adc8feac1a131c9e1c0cfd266277e0de4ae12d58 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 19 Jul 2024 14:04:16 +0900 Subject: [PATCH 10/15] =?UTF-8?q?two=20hands=E3=82=92ompl=E8=A8=AD?= =?UTF-8?q?=E5=AE=9A=E3=81=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/ompl_planning.yaml | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/sciurus17_moveit_config/config/ompl_planning.yaml b/sciurus17_moveit_config/config/ompl_planning.yaml index c700fa1..f56aa98 100644 --- a/sciurus17_moveit_config/config/ompl_planning.yaml +++ b/sciurus17_moveit_config/config/ompl_planning.yaml @@ -254,3 +254,32 @@ r_arm_waist_group: - 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 + From e96d86a6d0f9097b55ea9a1461aa38f510cde478 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 19 Jul 2024 14:04:51 +0900 Subject: [PATCH 11/15] =?UTF-8?q?MoveitConfigsBuilder=E5=91=A8=E8=BE=BA?= =?UTF-8?q?=E3=81=AE=E8=A8=98=E8=BC=89=E3=82=92=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_moveit_config/launch/run_move_group.launch.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index f58eae6..cceaf13 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -35,7 +35,12 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("sciurus17") - .planning_pipelines("ompl", ["ompl"]) + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .planning_pipelines(pipelines=["ompl"]) .to_moveit_configs() ) From 231905bad85eea41f78875bb2744081f5977135a Mon Sep 17 00:00:00 2001 From: chama1176 Date: Fri, 26 Jul 2024 17:09:45 +0900 Subject: [PATCH 12/15] add mock components support --- sciurus17_examples/README.md | 41 ++++++++++++++++++++---- sciurus17_examples/launch/demo.launch.py | 8 +++++ 2 files changed, 42 insertions(+), 7 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 58eb169..d9f87f2 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) @@ -63,6 +65,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ではカメラを使ったサンプルを実行することはできません。 + ## サンプルプログラムを実行する 準備ができたらサンプルプログラムを実行します。 @@ -86,13 +101,25 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ `demo.launch`を実行している状態で各サンプルを実行できます。 -- [gripper\_control](#gripper_control) -- [neck\_control](#neck_control) -- [waist\_control](#waist_control) -- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) -- [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) -- [head\_camera\_tracking](#head_camera_tracking) -- [chest\_camera\_tracking](#chest_camera_tracking) +- [sciurus17\_examples](#sciurus17_examples) + - [準備（実機を使う場合）](#準備実機を使う場合) + - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) + - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) + - [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) + - [gripper\_control](#gripper_control) + - [neck\_control](#neck_control) + - [waist\_control](#waist_control) + - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) + - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) + - [head\_camera\_tracking](#head_camera_tracking) + - [chest\_camera\_tracking](#chest_camera_tracking) 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 diff --git a/sciurus17_examples/launch/demo.launch.py b/sciurus17_examples/launch/demo.launch.py index ee446ef..8c73ec8 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, From 39382ca4f646634bb97a0fcbe06a140f0fab568c Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 25 Nov 2024 10:35:34 +0900 Subject: [PATCH 13/15] =?UTF-8?q?merge=E3=83=9F=E3=82=B9=E3=82=92=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 5 ----- sciurus17_examples/README.md | 29 ++++++++--------------------- 2 files changed, 8 insertions(+), 26 deletions(-) diff --git a/README.md b/README.md index 7e08161..8fadce3 100644 --- a/README.md +++ b/README.md @@ -46,9 +46,6 @@ ROS 2 package suite of Sciurus17. ## Installation ### Build from source -2024 July 18現在、カメラを使ったサンプルで使用しているusb_camを正常に実行するためには -aptに登録されているものよりも最新のimage_commonのバージョンを使用する必要があります -そのためsciurus17関連のパッケージと合わせてソースからビルドします ```sh # Setup ROS environment @@ -60,8 +57,6 @@ 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 -# To run examples with camera -git clone -b 5.1.4 https://github.com/ros-perception/image_common.git # Install dependencies rosdep install -r -y -i --from-paths . diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index e990e25..897830b 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -101,27 +101,14 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ ## Examples `demo.launch`を実行している状態で各サンプルを実行できます。 - -- [sciurus17\_examples](#sciurus17_examples) - - [準備（実機を使う場合）](#準備実機を使う場合) - - [1. Sciurus17本体をPCに接続する](#1-sciurus17本体をpcに接続する) - - [2. USB通信ポートの接続を確認する](#2-usb通信ポートの接続を確認する) - - [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) - - [gripper\_control](#gripper_control) - - [neck\_control](#neck_control) - - [waist\_control](#waist_control) - - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) - - [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) + - [gripper\_control](#gripper_control) + - [neck\_control](#neck_control) + - [waist\_control](#waist_control) + - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) + - [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`を付けて実行することで表示できます。 From 98a07fc688b445bc7c7c5d9da4f525328d47a0fb Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 25 Nov 2024 10:36:26 +0900 Subject: [PATCH 14/15] =?UTF-8?q?=E5=B7=AE=E5=88=86=E3=82=92=E6=B6=88?= =?UTF-8?q?=E3=81=99=E3=81=9F=E3=82=81=E3=81=AB=E6=94=B9=E8=A1=8C=E3=82=92?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 897830b..e6cbaa3 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -101,6 +101,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ ## Examples `demo.launch`を実行している状態で各サンプルを実行できます。 + - [gripper\_control](#gripper_control) - [neck\_control](#neck_control) - [waist\_control](#waist_control) From 00871cb999f8a991020e63b2f58b23b356c4e449 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 25 Nov 2024 10:37:01 +0900 Subject: [PATCH 15/15] fix indent --- sciurus17_examples/README.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index e6cbaa3..e585da4 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -102,14 +102,14 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ `demo.launch`を実行している状態で各サンプルを実行できます。 - - [gripper\_control](#gripper_control) - - [neck\_control](#neck_control) - - [waist\_control](#waist_control) - - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) - - [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) +- [gripper\_control](#gripper_control) +- [neck\_control](#neck_control) +- [waist\_control](#waist_control) +- [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) +- [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`を付けて実行することで表示できます。