diff --git a/README.md b/README.md index 83acd7c..6f0cd88 100644 --- a/README.md +++ b/README.md @@ -87,7 +87,6 @@ $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' - crane_plus_gazebo - [README](./crane_plus_gazebo/README.md) - CRANE+V2のGazeboシミュレーションパッケージです - - **現在、Gazebo上のCRANE+V2を制御することはできません** - crane_plus_moveit_config - [README](./crane_plus_moveit_config/README.md) - CRANE+V2の`moveit2`設定ファイルです diff --git a/crane_plus_description/package.xml b/crane_plus_description/package.xml index 756f266..8f0f0d1 100644 --- a/crane_plus_description/package.xml +++ b/crane_plus_description/package.xml @@ -11,6 +11,7 @@ ament_cmake + gazebo_ros2_control joint_state_publisher_gui robot_state_publisher rviz2 diff --git a/crane_plus_description/urdf/crane_plus.gazebo.xacro b/crane_plus_description/urdf/crane_plus.gazebo.xacro new file mode 100644 index 0000000..e015b9e --- /dev/null +++ b/crane_plus_description/urdf/crane_plus.gazebo.xacro @@ -0,0 +1,56 @@ + + + + + + + + gazebo_ros2_control/GazeboSystem + $(find crane_plus_control)/config/crane_plus_controllers.yaml + + + + + 0.2 + 0.2 + Gazebo/Black + + + + 0.2 + 0.2 + Gazebo/White + + + + 0.2 + 0.2 + Gazebo/Black + + + + 0.2 + 0.2 + Gazebo/White + + + + 0.2 + 0.2 + Gazebo/Black + + + + 0.2 + 0.2 + Gazebo/White + + + + \ No newline at end of file diff --git a/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro b/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro new file mode 100644 index 0000000..b24f188 --- /dev/null +++ b/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro @@ -0,0 +1,79 @@ + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + ${joint_1_lower_limit} + ${joint_1_upper_limit} + + + + + + + + + ${joint_2_lower_limit} + ${joint_2_upper_limit} + + + + + + + + + ${joint_3_lower_limit} + ${joint_3_upper_limit} + + + + + + + + + ${joint_4_lower_limit} + ${joint_4_upper_limit} + + + + + + + + + ${joint_hand_lower_limit} + ${joint_hand_upper_limit} + + + + + + + + + \ No newline at end of file diff --git a/crane_plus_description/urdf/crane_plus.urdf.xacro b/crane_plus_description/urdf/crane_plus.urdf.xacro index ccea6d2..051f279 100644 --- a/crane_plus_description/urdf/crane_plus.urdf.xacro +++ b/crane_plus_description/urdf/crane_plus.urdf.xacro @@ -9,6 +9,10 @@ + + + + @@ -79,22 +83,52 @@ - + + + + + + + + + diff --git a/crane_plus_examples/README.md b/crane_plus_examples/README.md index a129b78..b4bc7c4 100644 --- a/crane_plus_examples/README.md +++ b/crane_plus_examples/README.md @@ -2,7 +2,9 @@ このパッケージはCRANE+V2 ROS 2パッケージのサンプルコード集です。 -## 実行手順 +## 準備（実機を使う場合） + +![crane_plus](https://rt-net.github.io/images/crane-plus/CRANEV2-500x500.png) ### 1. CRANE+V2本体をPCに接続する @@ -28,18 +30,29 @@ controller (`crane_plus_control`)を起動します。 $ ros2 launch crane_plus_examples demo.launch.py ``` -### 4. サンプルプログラムを実行する +## 準備（Gazeboを使う場合） + +![crane_plus_gazebo](https://rt-net.github.io/images/crane-plus/crane_plus_gazebo.png) + +### 1. move_groupとgazeboを起動する + +次のコマンドでmove_group (`crane_plus_moveit_config`)と +Gazeboを起動します。 + +```sh +$ ros2 launch crane_plus_gazebo crane_plus_gazebo.launch.py +``` + +## サンプルプログラムを実行する -サンプルプログラムを実行します。 +準備ができたらサンプルプログラムを実行します。 例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 ```sh $ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' ``` -### 5. サンプルプログラムを終了する - -`Ctrl+c`を入力してサンプルプログラムを終了します。 +終了するときは`Ctrl+c`を入力します。 ## Examples diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp index 9dd50e3..94d452d 100644 --- a/crane_plus_examples/src/pick_and_place.cpp +++ b/crane_plus_examples/src/pick_and_place.cpp @@ -57,7 +57,7 @@ int main(int argc, char ** argv) auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); double GRIPPER_DEFAULT = 0.0; double GRIPPER_OPEN = to_radians(-30); - double GRIPPER_CLOSE = to_radians(10); + double GRIPPER_CLOSE = to_radians(-5); move_group_arm.setNamedTarget("vertical"); move_group_arm.move(); @@ -89,11 +89,19 @@ int main(int argc, char ** argv) move_group_arm.setPoseTarget(target_pose); move_group_arm.move(); + target_pose.position.z = 0.15; + move_group_arm.setPoseTarget(target_pose); + move_group_arm.move(); + // Grasp gripper_joint_values[0] = GRIPPER_CLOSE; move_group_gripper.setJointValueTarget(gripper_joint_values); move_group_gripper.move(); + target_pose.position.z = 0.17; + move_group_arm.setPoseTarget(target_pose); + move_group_arm.move(); + // ----- Placing Preparation ----- move_group_arm.setNamedTarget("home"); move_group_arm.move(); diff --git a/crane_plus_gazebo/README.md b/crane_plus_gazebo/README.md index d8e653d..24a7b79 100644 --- a/crane_plus_gazebo/README.md +++ b/crane_plus_gazebo/README.md @@ -2,18 +2,13 @@ CRANE+V2 のGazeboシミュレーションパッケージです。 -**現在、Gazebo上のCRANE+V2を動かすことは出来ません** - -`crane_plus_gazebo`は -[gazebo_ros2_control](https://github.com/ros-simulation/gazebo_ros2_control)及び -[ros2_control](https://github.com/ros-controls/ros2_control) -の開発状況に依存しています。 - ## ノードの起動 -次のコマンドを実行するとGazeboが起動し、CRANE+V2モデルと、Table、Cubeが表示がされます。 +次のコマンドを実行するとGazeboが起動し、CRANE+V2モデルとTableが表示されます。 + +初回起動時はTableのモデルをダウンロードするため、モデルの表示に時間がかかることがあります。 -初回起動時はTableとCubeのモデルをダウンロードするため、モデルの表示に時間がかかることがあります。 +実機との接続や`crane_plus_examples/launch/demo.launch/py`の実行は必要ありません。 ```sh $ ros2 launch crane_plus_gazebo crane_plus_gazebo.launch.py diff --git a/crane_plus_gazebo/launch/crane_plus_gazebo.launch.py b/crane_plus_gazebo/launch/crane_plus_gazebo.launch.py index 9a15c6f..0a2b014 100644 --- a/crane_plus_gazebo/launch/crane_plus_gazebo.launch.py +++ b/crane_plus_gazebo/launch/crane_plus_gazebo.launch.py @@ -17,6 +17,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import ExecuteProcess from launch.actions import IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -57,6 +58,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource([ get_package_share_directory('crane_plus_moveit_config'), '/launch/run_move_group.launch.py']), + launch_arguments={'xacro_use_gazebo': 'true'}.items(), ) spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py', @@ -64,6 +66,24 @@ def generate_launch_description(): '-z', '1.02', '-topic', '/robot_description'], output='screen') + spawn_joint_state_controller = ExecuteProcess( + cmd=['ros2 run controller_manager spawner.py joint_state_controller'], + shell=True, + output='screen', + ) + + spawn_arm_controller = ExecuteProcess( + cmd=['ros2 run controller_manager spawner.py crane_plus_arm_controller'], + shell=True, + output='screen', + ) + + spawn_gripper_controller = ExecuteProcess( + cmd=['ros2 run controller_manager spawner.py crane_plus_gripper_controller'], + shell=True, + output='screen', + ) + return LaunchDescription([ declare_arg_gui, declare_arg_server, @@ -71,4 +91,7 @@ def generate_launch_description(): gzclient, move_group, spawn_entity, + spawn_joint_state_controller, + spawn_arm_controller, + spawn_gripper_controller ]) diff --git a/crane_plus_gazebo/worlds/table.world b/crane_plus_gazebo/worlds/table.world index 1548816..39718d7 100644 --- a/crane_plus_gazebo/worlds/table.world +++ b/crane_plus_gazebo/worlds/table.world @@ -5,6 +5,12 @@ 0 + + + 0.75 -0.75 1.4 0 0.29 2.21 + + + model://sun @@ -14,17 +20,16 @@ model://table - 0.29 0 0 0 0 1.5708 - + diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py index bf05c6e..5cba8cb 100644 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -17,8 +17,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -import xacro import yaml @@ -49,11 +50,15 @@ def load_yaml(package_name, file_path): def generate_launch_description(): # planning_context + declare_xacro_use_gazebo = DeclareLaunchArgument( + 'xacro_use_gazebo', + default_value='false', + description='Set "true" to enable gazebo_ros2_control plugin.' + ) xacro_file = os.path.join(get_package_share_directory('crane_plus_description'), 'urdf', 'crane_plus.urdf.xacro') - doc = xacro.process_file(xacro_file) - robot_description_config = doc.toprettyxml(indent=' ') - robot_description = {'robot_description': robot_description_config} + robot_description = {'robot_description': Command( + ['xacro ', xacro_file, ' use_gazebo:=', LaunchConfiguration('xacro_use_gazebo')])} robot_description_semantic_config = load_file( 'crane_plus_moveit_config', 'config/crane_plus.srdf') @@ -91,12 +96,6 @@ def generate_launch_description(): 'publish_state_updates': True, 'publish_transforms_updates': True} - # Declare launch arguments - declare_arg_controller = DeclareLaunchArgument( - 'controller', - default_value='true', - description='Set to "true" to launch crane_plus_controller.') - # Start the actual move_group node/action server run_move_group_node = Node(package='moveit_ros_move_group', executable='move_group', @@ -136,7 +135,7 @@ def generate_launch_description(): output='both', parameters=[robot_description]) - return LaunchDescription([declare_arg_controller, + return LaunchDescription([declare_xacro_use_gazebo, run_move_group_node, rviz_node, static_tf,