Merge remote-tracking branch 'origin/ros2' into feature/moveit_py

.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:

README.md

@@ -57,14 +57,15 @@ 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
 git clone -b 5.1.4 https://github.com/ros-perception/image_common.git
 
 # Install dependencies
 rosdep install -r -y -i --from-paths .
+sudo apt install libpcl-dev
 
 # Build & Install
 cd ~/ros2_ws

sciurus17/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>sciurus17</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>ROS 2 package suite of Sciurus17</description>
   <maintainer email="[email protected]">RT Corporation</maintainer>
 

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():

sciurus17_control/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>sciurus17_control</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>The Sciurus17 control package</description>
   <maintainer email="[email protected]">RT Corporation</maintainer>
   <license>Apache License 2.0</license>

sciurus17_examples/CMakeLists.txt

@@ -24,6 +24,8 @@ find_package(control_msgs REQUIRED)
 find_package(image_geometry REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(OpenCV REQUIRED COMPONENTS core)
+find_package(pcl_conversions REQUIRED)
+find_package(pcl_ros REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(rclcpp_action REQUIRED)
@@ -115,6 +117,8 @@ set(executable_list
   waist_control
   pick_and_place_right_arm_waist
   pick_and_place_left_arm
+  pick_and_place_tf
+  point_cloud_detection
 )
 foreach(loop_var IN LISTS executable_list)
   add_executable(${loop_var} src/${loop_var}.cpp)
@@ -126,6 +130,8 @@ foreach(loop_var IN LISTS executable_list)
     image_geometry
     moveit_ros_planning_interface
     OpenCV
+    pcl_ros
+    pcl_conversions
     rclcpp
     tf2_geometry_msgs
     control_msgs

sciurus17_examples/README.md

@@ -21,6 +21,7 @@
     - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm)
     - [head\_camera\_tracking](#head_camera_tracking)
     - [chest\_camera\_tracking](#chest_camera_tracking)
+    - [point\_cloud\_detection](#point_cloud_detection)
 
 ## 準備（実機を使う場合）
 
@@ -101,25 +102,14 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_
 
 `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)
+- [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`を付けて実行することで表示できます。
 
@@ -230,3 +220,21 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py
 [back to example list](#examples)
 
 ---
+
+### point_cloud_detection
+
+点群から物体を検出して掴むコード例です。
+
+検出された物体位置はtfのフレームとして配信されます。
+tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。
+掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。
+物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。
+
+次のコマンドを実行します
+```sh
+ros2 launch sciurus17_examples camera_example.launch.py example:='point_cloud_detection'
+```
+
+[back to example list](#examples)
+
+---

sciurus17_examples/launch/camera_example.launch.py

@@ -0,0 +1,89 @@
+# Copyright 2024 RT Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+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
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, 'r') as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    description_loader = RobotDescriptionLoader()
+
+    robot_description_semantic_config = load_file(
+        'sciurus17_moveit_config', 'config/sciurus17.srdf')
+    robot_description_semantic = {
+        'robot_description_semantic': robot_description_semantic_config}
+
+    kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml')
+
+    declare_example_name = DeclareLaunchArgument(
+        'example', default_value='point_cloud_detection',
+        description=('Set an example executable name: '
+                     '[point_cloud_detection]')
+    )
+
+    declare_use_sim_time = DeclareLaunchArgument(
+        'use_sim_time', default_value='false',
+        description=('Set true when using the gazebo simulator.')
+    )
+
+    picking_node = Node(name='pick_and_place_tf',
+                        package='sciurus17_examples',
+                        executable='pick_and_place_tf',
+                        output='screen',
+                        parameters=[{'robot_description': description_loader.load()},
+                                    robot_description_semantic,
+                                    kinematics_yaml])
+
+    detection_node = Node(name=[LaunchConfiguration('example'), '_node'],
+                          package='sciurus17_examples',
+                          executable=LaunchConfiguration('example'),
+                          output='screen')
+
+    return LaunchDescription([
+        declare_use_sim_time,
+        SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')),
+        picking_node,
+        declare_example_name,
+        detection_node
+    ])

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():

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
 
 

sciurus17_examples/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>sciurus17_examples</name>
-  <version>3.0.0</version>
+  <version>3.1.0</version>
   <description>examples of Sciurus17 ROS package</description>
 
   <maintainer email="[email protected]">RT Corporation</maintainer>
@@ -24,6 +24,7 @@
   <depend>image_geometry</depend>
   <depend>libopencv-dev</depend>
   <depend>moveit_ros_planning_interface</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tf2_geometry_msgs</depend>