From 7fdbb478f5dcc44fbe624fb11c98c4609347d08c Mon Sep 17 00:00:00 2001 From: chama1176 Date: Mon, 8 Jul 2024 16:39:17 +0900 Subject: [PATCH 01/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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/18] =?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 e741c8f6ef2dbb3ac1e2f67831b9bd25fc47ea3a Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 13 Nov 2024 11:10:51 +0900 Subject: [PATCH 12/18] Update README.md Co-authored-by: Kuwamai --- README.md | 3 --- 1 file changed, 3 deletions(-) diff --git a/README.md b/README.md index 3c29212..c8fc4ba 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 From 33344ef29d601467911add407b3ec2ee51645399 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 13 Nov 2024 11:11:11 +0900 Subject: [PATCH 13/18] Update sciurus17_moveit_config/launch/run_move_group.launch.py Co-authored-by: Kuwamai --- sciurus17_moveit_config/launch/run_move_group.launch.py | 1 - 1 file changed, 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 cceaf13..bfdd568 100644 --- a/sciurus17_moveit_config/launch/run_move_group.launch.py +++ b/sciurus17_moveit_config/launch/run_move_group.launch.py @@ -39,7 +39,6 @@ def generate_launch_description(): 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 57d513c99aac956e94ebda930e28da5367200ee5 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 13 Nov 2024 11:11:21 +0900 Subject: [PATCH 14/18] Update README.md Co-authored-by: Kuwamai --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index c8fc4ba..1efbfd6 100644 --- a/README.md +++ b/README.md @@ -54,7 +54,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/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 39058b94e70e953d3f260536808d4de97f80d669 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 13 Nov 2024 11:12:03 +0900 Subject: [PATCH 15/18] Update README.md Co-authored-by: Kuwamai --- README.md | 2 -- 1 file changed, 2 deletions(-) diff --git a/README.md b/README.md index 1efbfd6..b678696 100644 --- a/README.md +++ b/README.md @@ -57,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 . From 26abaebd68e10483b7cb4523ee3ccb450891f0b0 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Wed, 13 Nov 2024 16:13:31 +0900 Subject: [PATCH 16/18] =?UTF-8?q?README=E3=81=AB=E7=82=B9=E7=BE=A4?= =?UTF-8?q?=E6=A4=9C=E5=87=BA=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E4=BE=9D=E5=AD=98=E9=96=A2=E4=BF=82=E3=81=AB=E8=BF=BD=E8=A8=98?= 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 b678696..8fadce3 100644 --- a/README.md +++ b/README.md @@ -60,6 +60,7 @@ 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 From a8d0a83131473396c9f0cfbc452ad2efc0bf1263 Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 21 Nov 2024 13:29:09 +0900 Subject: [PATCH 17/18] add dependency in ci --- .github/workflows/industrial_ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index f951dcf..8993dd3 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -11,7 +11,7 @@ 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: From 94a7527f4dd23a3067162c4aaae38a4802c8ff4a Mon Sep 17 00:00:00 2001 From: chama1176 Date: Thu, 21 Nov 2024 14:37:08 +0900 Subject: [PATCH 18/18] =?UTF-8?q?ci=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 --- .../launch/sciurus17_control.launch.py | 2 +- sciurus17_examples/launch/camera_example.launch.py | 6 +++--- sciurus17_examples/launch/demo.launch.py | 2 +- sciurus17_examples/launch/example.launch.py | 4 ++-- .../launch/sciurus17_with_table.launch.py | 4 ++-- .../launch/run_move_group.launch.py | 13 +++++++------ .../launch/setup_assistant.launch.py | 3 ++- 7 files changed, 18 insertions(+), 16 deletions(-) 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_gazebo/launch/sciurus17_with_table.launch.py b/sciurus17_gazebo/launch/sciurus17_with_table.launch.py index 2fe2559..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(): diff --git a/sciurus17_moveit_config/launch/run_move_group.launch.py b/sciurus17_moveit_config/launch/run_move_group.launch.py index bfdd568..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,12 +35,12 @@ def generate_launch_description(): ) moveit_config = ( - MoveItConfigsBuilder("sciurus17") + MoveItConfigsBuilder('sciurus17') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) - .planning_pipelines(pipelines=["ompl"]) + .planning_pipelines(pipelines=['ompl']) .to_moveit_configs() ) @@ -48,7 +49,7 @@ def generate_launch_description(): } moveit_config.move_group_capabilities = { - "capabilities": "" + 'capabilities': '' } # Move group 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)