From 587ec53806889dbdc09029e5007a66adbdcc2ac4 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 19 Jul 2024 11:21:01 +0900 Subject: [PATCH 001/111] =?UTF-8?q?jazzy=E5=AF=BE=E5=BF=9C=EF=BC=88MoveitC?= =?UTF-8?q?onfigsBuilder=EF=BC=89=20*=20MoveitCofigsBuilder=E3=82=92?= =?UTF-8?q?=E4=BD=BF=E3=81=A3=E3=81=A6moveit=5Fconfig=E3=81=AE=E6=9E=A0?= =?UTF-8?q?=E3=82=92=E4=BD=9C=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * planning_plugins, requesr_adaptorsの設定をompl_planning.yamlの先頭で行う * ompl_planning.yamlにstart_state_max_bounds_errorの設定を追加 * 7/19ここまで * MoveitConfigsBuilderを使うに伴った、yamlファイルの指定の項目追加 * 一通りMoveItConfigsBuilderに対応 * MoveIt Setup Assistantを使って設定ファイルを生成 * urdfの指定を追加 * crane_plus_configのrun_move_group.launchを呼ぶ * Unable to parse SRDFのエラーが解決できない * demo.launchが起動できるようになった * 余計なコメントアウト部分削除 * 余計なコメントアウト削除 * rviz上でMottionPlanningができるようになった * 余計なコメントアウト削除 --- crane_plus_description/package.xml | 5 +- crane_plus_examples/launch/demo.launch.py | 5 +- crane_plus_examples/src/aruco_detection.cpp | 3 +- crane_plus_examples/src/color_detection.cpp | 6 +- crane_plus_moveit_config/CHANGELOG.rst | 0 crane_plus_moveit_config/CMakeLists.txt | 20 +- crane_plus_moveit_config/README.md | 0 .../config/controllers.yaml | 41 +- .../config/crane_plus.srdf | 41 +- .../config/joint_limits.yaml | 40 ++ .../config/kinematics.yaml | 4 +- crane_plus_moveit_config/config/moveit.rviz | 51 +++ .../config/ompl_planning.yaml | 16 + .../config/pilz_cartesian_limits.yaml | 6 + .../config/sensors_3d.yaml | 23 + .../launch/demo.launch.py | 7 + .../launch/move_group.launch.py | 7 + .../launch/moveit_rviz.launch.py | 7 + crane_plus_moveit_config/launch/rsp.launch.py | 7 + .../launch/run_move_group.launch.py | 190 ++++---- .../launch/run_move_group.rviz | 0 .../launch/setup_assistant.launch.py | 7 + .../launch/spawn_controllers.launch.py | 7 + .../launch/static_virtual_joint_tfs.launch.py | 7 + .../launch/warehouse_db.launch.py | 7 + crane_plus_moveit_config/package.xml | 50 ++- install/COLCON_IGNORE | 0 install/_local_setup_util_ps1.py | 407 ++++++++++++++++++ install/_local_setup_util_sh.py | 407 ++++++++++++++++++ .../crane_plus_moveit_config | 1 + .../packages/crane_plus_moveit_config | 0 .../crane_plus_moveit_config | 1 + .../packages/crane_plus_moveit_config | 1 + .../crane_plus_moveit_config/.setup_assistant | 10 + ...ane_plus_moveit_configConfig-version.cmake | 14 + .../crane_plus_moveit_configConfig.cmake | 42 ++ .../config/controllers.yaml | 27 ++ .../config/crane_plus.srdf | 42 ++ .../config/joint_limits.yaml | 35 ++ .../config/kinematics.yaml | 4 + .../config/moveit.rviz | 51 +++ .../config/ompl_planning.yaml | 196 +++++++++ .../config/pilz_cartesian_limits.yaml | 6 + .../config/sensors_3d.yaml | 23 + .../environment/ament_prefix_path.dsv | 1 + .../environment/ament_prefix_path.sh | 4 + .../environment/path.dsv | 1 + .../environment/path.sh | 5 + .../hook/cmake_prefix_path.dsv | 1 + .../hook/cmake_prefix_path.ps1 | 3 + .../hook/cmake_prefix_path.sh | 3 + .../launch/demo.launch.py | 7 + .../launch/move_group.launch.py | 7 + .../launch/moveit_rviz.launch.py | 7 + .../launch/rsp.launch.py | 7 + .../launch/run_move_group.launch.py | 237 ++++++++++ .../launch/run_move_group.rviz | 351 +++++++++++++++ .../launch/setup_assistant.launch.py | 7 + .../launch/spawn_controllers.launch.py | 7 + .../launch/static_virtual_joint_tfs.launch.py | 7 + .../launch/warehouse_db.launch.py | 7 + .../crane_plus_moveit_config/local_setup.bash | 46 ++ .../crane_plus_moveit_config/local_setup.dsv | 2 + .../crane_plus_moveit_config/local_setup.sh | 184 ++++++++ .../crane_plus_moveit_config/local_setup.zsh | 59 +++ .../crane_plus_moveit_config/package.bash | 39 ++ .../crane_plus_moveit_config/package.dsv | 8 + .../crane_plus_moveit_config/package.ps1 | 116 +++++ .../share/crane_plus_moveit_config/package.sh | 87 ++++ .../crane_plus_moveit_config/package.xml | 51 +++ .../crane_plus_moveit_config/package.zsh | 50 +++ install/local_setup.bash | 121 ++++++ install/local_setup.ps1 | 55 +++ install/local_setup.sh | 137 ++++++ install/local_setup.zsh | 134 ++++++ install/setup.bash | 34 ++ install/setup.ps1 | 30 ++ install/setup.sh | 49 +++ install/setup.zsh | 34 ++ log/COLCON_IGNORE | 0 log/latest | 1 + log/latest_build | 1 + 82 files changed, 3559 insertions(+), 165 deletions(-) mode change 100644 => 100755 crane_plus_moveit_config/CHANGELOG.rst mode change 100644 => 100755 crane_plus_moveit_config/CMakeLists.txt mode change 100644 => 100755 crane_plus_moveit_config/README.md mode change 100644 => 100755 crane_plus_moveit_config/config/controllers.yaml mode change 100644 => 100755 crane_plus_moveit_config/config/crane_plus.srdf create mode 100755 crane_plus_moveit_config/config/joint_limits.yaml mode change 100644 => 100755 crane_plus_moveit_config/config/kinematics.yaml create mode 100644 crane_plus_moveit_config/config/moveit.rviz mode change 100644 => 100755 crane_plus_moveit_config/config/ompl_planning.yaml create mode 100644 crane_plus_moveit_config/config/pilz_cartesian_limits.yaml create mode 100644 crane_plus_moveit_config/config/sensors_3d.yaml create mode 100644 crane_plus_moveit_config/launch/demo.launch.py create mode 100644 crane_plus_moveit_config/launch/move_group.launch.py create mode 100644 crane_plus_moveit_config/launch/moveit_rviz.launch.py create mode 100644 crane_plus_moveit_config/launch/rsp.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/run_move_group.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/run_move_group.rviz create mode 100644 crane_plus_moveit_config/launch/setup_assistant.launch.py create mode 100644 crane_plus_moveit_config/launch/spawn_controllers.launch.py create mode 100644 crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py create mode 100644 crane_plus_moveit_config/launch/warehouse_db.launch.py mode change 100644 => 100755 crane_plus_moveit_config/package.xml create mode 100644 install/COLCON_IGNORE create mode 100644 install/_local_setup_util_ps1.py create mode 100644 install/_local_setup_util_sh.py create mode 100644 install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config create mode 100644 install/crane_plus_moveit_config/share/ament_index/resource_index/packages/crane_plus_moveit_config create mode 100644 install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config create mode 100644 install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml create mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh create mode 100644 install/local_setup.bash create mode 100644 install/local_setup.ps1 create mode 100644 install/local_setup.sh create mode 100644 install/local_setup.zsh create mode 100644 install/setup.bash create mode 100644 install/setup.ps1 create mode 100644 install/setup.sh create mode 100644 install/setup.zsh create mode 100644 log/COLCON_IGNORE create mode 120000 log/latest create mode 120000 log/latest_build diff --git a/crane_plus_description/package.xml b/crane_plus_description/package.xml index cf986cda..e8805be6 100644 --- a/crane_plus_description/package.xml +++ b/crane_plus_description/package.xml @@ -13,8 +13,9 @@ ament_cmake ament_cmake_python - gazebo_ros2_control - ign_ros2_control + + gz_ros2_control joint_state_publisher_gui launch robot_state_publisher diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 12a7dbeb..16e3bd93 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -30,7 +30,6 @@ def generate_launch_description(): default_value='/dev/ttyUSB0', description='Set port name.' ) - declare_use_camera = DeclareLaunchArgument( 'use_camera', default_value='false', @@ -97,7 +96,7 @@ def generate_launch_description(): declare_use_camera, declare_video_device, move_group, - move_group_camera, + # move_group_camera, control_node, - usb_cam_node + # usb_cam_node ]) diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index bdf9b19d..926726a6 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -27,7 +27,8 @@ #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" #include "opencv2/aruco.hpp" -#include "cv_bridge/cv_bridge.h" +// #include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_ros/transform_broadcaster.h" diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp index 8590e51e..5e274136 100644 --- a/crane_plus_examples/src/color_detection.cpp +++ b/crane_plus_examples/src/color_detection.cpp @@ -29,8 +29,10 @@ #include "tf2_ros/transform_broadcaster.h" #include "opencv2/opencv.hpp" #include "opencv2/imgproc/imgproc.hpp" -#include "cv_bridge/cv_bridge.h" -#include "image_geometry/pinhole_camera_model.h" +// #include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" +// #include "image_geometry/pinhole_camera_model.h" +#include "image_geometry/pinhole_camera_model.hpp" using std::placeholders::_1; class ImageSubscriber : public rclcpp::Node diff --git a/crane_plus_moveit_config/CHANGELOG.rst b/crane_plus_moveit_config/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/CMakeLists.txt b/crane_plus_moveit_config/CMakeLists.txt old mode 100644 new mode 100755 index b9b9f1f6..895a3c1e --- a/crane_plus_moveit_config/CMakeLists.txt +++ b/crane_plus_moveit_config/CMakeLists.txt @@ -1,17 +1,13 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.22) project(crane_plus_moveit_config) find_package(ament_cmake REQUIRED) -install(DIRECTORY - launch - config - DESTINATION share/${PROJECT_NAME}/ -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/crane_plus_moveit_config/README.md b/crane_plus_moveit_config/README.md old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/config/controllers.yaml b/crane_plus_moveit_config/config/controllers.yaml old mode 100644 new mode 100755 index 4625155f..f30686de --- a/crane_plus_moveit_config/config/controllers.yaml +++ b/crane_plus_moveit_config/config/controllers.yaml @@ -1,20 +1,25 @@ -controller_names: - - crane_plus_arm_controller - - crane_plus_gripper_controller +# MoveIt uses this configuration for controller management +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager -crane_plus_arm_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - crane_plus_joint1 - - crane_plus_joint2 - - crane_plus_joint3 - - crane_plus_joint4 +moveit_simple_controller_manager: + controller_names: + - crane_plus_arm_controller + - crane_plus_gripper_controller + + crane_plus_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - crane_plus_joint1 + - crane_plus_joint2 + - crane_plus_joint3 + - crane_plus_joint4 + + crane_plus_gripper_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - crane_plus_joint_hand -crane_plus_gripper_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - crane_plus_joint_hand diff --git a/crane_plus_moveit_config/config/crane_plus.srdf b/crane_plus_moveit_config/config/crane_plus.srdf old mode 100644 new mode 100755 index 7e3685c5..af40281f --- a/crane_plus_moveit_config/config/crane_plus.srdf +++ b/crane_plus_moveit_config/config/crane_plus.srdf @@ -1,43 +1,42 @@ - + - + - + - - + + - - - - + + + + - - - - + + + + - + - + - - - - - - + + + + + diff --git a/crane_plus_moveit_config/config/joint_limits.yaml b/crane_plus_moveit_config/config/joint_limits.yaml new file mode 100755 index 00000000..ac0df2e2 --- /dev/null +++ b/crane_plus_moveit_config/config/joint_limits.yaml @@ -0,0 +1,40 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + crane_plus_joint1: + has_velocity_limits: true + max_velocity: 2.0 + # has_acceleration_limits: false + has_acceleration_limits: true + max_acceleration: 1.0 + crane_plus_joint2: + has_velocity_limits: true + max_velocity: 2.0 + # has_acceleration_limits: false + has_acceleration_limits: true + max_acceleration: 1.0 + crane_plus_joint3: + has_velocity_limits: true + max_velocity: 2.0 + # has_acceleration_limits: false + has_acceleration_limits: true + max_acceleration: 1.0 + crane_plus_joint4: + has_velocity_limits: true + max_velocity: 2.0 + # has_acceleration_limits: false + has_acceleration_limits: true + max_acceleration: 1.0 + crane_plus_joint_hand: + has_velocity_limits: true + max_velocity: 2.0 + # has_acceleration_limits: false + has_acceleration_limits: true + max_acceleration: 1.0 \ No newline at end of file diff --git a/crane_plus_moveit_config/config/kinematics.yaml b/crane_plus_moveit_config/config/kinematics.yaml old mode 100644 new mode 100755 index a07a406d..a1f17d08 --- a/crane_plus_moveit_config/config/kinematics.yaml +++ b/crane_plus_moveit_config/config/kinematics.yaml @@ -1,5 +1,5 @@ arm: - kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin - kinematics_solver_attempts: 10 + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/crane_plus_moveit_config/config/moveit.rviz b/crane_plus_moveit_config/config/moveit.rviz new file mode 100644 index 00000000..f31651ed --- /dev/null +++ b/crane_plus_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/crane_plus_moveit_config/config/ompl_planning.yaml b/crane_plus_moveit_config/config/ompl_planning.yaml old mode 100644 new mode 100755 index 2e197f5a..3e0299c2 --- a/crane_plus_moveit_config/config/ompl_planning.yaml +++ b/crane_plus_moveit_config/config/ompl_planning.yaml @@ -1,3 +1,19 @@ +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 + +start_state_max_bounds_error: 0.1 + planner_configs: SBLkConfigDefault: type: geometric::SBL diff --git a/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml b/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/crane_plus_moveit_config/config/sensors_3d.yaml b/crane_plus_moveit_config/config/sensors_3d.yaml new file mode 100644 index 00000000..5d90e51d --- /dev/null +++ b/crane_plus_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,23 @@ +sensors: + - kinect_pointcloud + - kinect_depthimage +kinect_pointcloud: + filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater +kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 \ No newline at end of file diff --git a/crane_plus_moveit_config/launch/demo.launch.py b/crane_plus_moveit_config/launch/demo.launch.py new file mode 100644 index 00000000..2b134ee0 --- /dev/null +++ b/crane_plus_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/move_group.launch.py b/crane_plus_moveit_config/launch/move_group.launch.py new file mode 100644 index 00000000..d05ee6c5 --- /dev/null +++ b/crane_plus_moveit_config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/moveit_rviz.launch.py b/crane_plus_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 00000000..9034635d --- /dev/null +++ b/crane_plus_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/rsp.launch.py b/crane_plus_moveit_config/launch/rsp.launch.py new file mode 100644 index 00000000..28439b32 --- /dev/null +++ b/crane_plus_moveit_config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py old mode 100644 new mode 100755 index 7e442baf..bea9340b --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -20,19 +20,35 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node import yaml +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.launch_utils import ( + add_debuggable_node, + DeclareBooleanLaunchArg, +) # Reference: https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes/ # run_move_group/launch/run_move_group.launch.py + 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: + with open(absolute_file_path, "r") as file: return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except ( + EnvironmentError + ): # parent of IOError, OSError *and* WindowsError where available return None @@ -41,107 +57,99 @@ def load_yaml(package_name, file_path): absolute_file_path = os.path.join(package_path, file_path) try: - with open(absolute_file_path, 'r') as file: + with open(absolute_file_path, "r") as file: return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except ( + EnvironmentError + ): # parent of IOError, OSError *and* WindowsError where available return None def generate_launch_description(): + ld = LaunchDescription() + declare_loaded_description = DeclareLaunchArgument( - 'loaded_description', - default_value='', - description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.' + "loaded_description", + default_value="", + description="Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description.", ) - declare_rviz_config_file = DeclareLaunchArgument( - 'rviz_config_file', - default_value=get_package_share_directory( - 'crane_plus_moveit_config') + '/launch/run_move_group.rviz', - description='Set the path to rviz configuration file.' + ld.add_action(declare_loaded_description) + + # MoveItConfigBuilderによるパラメータ設定 + moveit_config = ( + # ロボット名の定義 + MoveItConfigsBuilder("crane_plus") + # URDFの設定 + .robot_description( + file_path=os.path.join( + get_package_share_directory("crane_plus_description"), + "urdf", + "crane_plus.urdf.xacro", + ), + mappings={}, + ) + # SRDFの設定 + .robot_description_semantic( + file_path="config/crane_plus.srdf", + mappings={"model": "crane_plus"}, + ) + .joint_limits(file_path="config/joint_limits.yaml") + # Planning Sceneのトピックの設定 + .planning_scene_monitor( + publish_planning_scene=True, + publish_geometry_updates=True, + publish_state_updates=True, + publish_transforms_updates=True, + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + # 軌道追従制御ノードの設定 + .trajectory_execution( + file_path="config/controllers.yaml", + moveit_manage_controllers=True + ) + # 軌道計画のプラグイン設定 + .planning_pipelines(pipelines=["ompl"], default_planning_pipeline="ompl") + # キネマティクスの設定 + .robot_description_kinematics(file_path="config/kinematics.yaml") + .to_moveit_configs() ) - robot_description = {'robot_description': LaunchConfiguration('loaded_description')} - - robot_description_semantic_config = load_file( - 'crane_plus_moveit_config', 'config/crane_plus.srdf') - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_config} - - kinematics_yaml = load_yaml('crane_plus_moveit_config', 'config/kinematics.yaml') - - # Planning Functionality - ompl_planning_pipeline_config = {'move_group': { - 'planning_plugin': 'ompl_interface/OMPLPlanner', - 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization \ - default_planner_request_adapters/FixWorkspaceBounds \ - default_planner_request_adapters/FixStartStateBounds \ - default_planner_request_adapters/FixStartStateCollision \ - default_planner_request_adapters/FixStartStatePathConstraints', - 'start_state_max_bounds_error': 0.1}} - ompl_planning_yaml = load_yaml('crane_plus_moveit_config', 'config/ompl_planning.yaml') - ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - controllers_yaml = load_yaml('crane_plus_moveit_config', 'config/controllers.yaml') - moveit_controllers = { - 'moveit_simple_controller_manager': controllers_yaml, - 'moveit_controller_manager': - 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} - - trajectory_execution = {'moveit_manage_controllers': True, - 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - 'trajectory_execution.allowed_goal_duration_margin': 0.5, - 'trajectory_execution.allowed_start_tolerance': 0.1} - - planning_scene_monitor_parameters = {'publish_planning_scene': True, - 'publish_geometry_updates': True, - 'publish_state_updates': True, - 'publish_transforms_updates': True} - - # Start the actual move_group node/action server - run_move_group_node = Node(package='moveit_ros_move_group', - executable='move_group', - output='screen', - parameters=[robot_description, - robot_description_semantic, - kinematics_yaml, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters]) + moveit_config.robot_description = { + "robot_description": LaunchConfiguration("loaded_description") + } - # RViz - rviz_config_file = LaunchConfiguration('rviz_config_file') - rviz_node = Node(package='rviz2', - executable='rviz2', - name='rviz2', - output='log', - arguments=['-d', rviz_config_file], - parameters=[robot_description, - robot_description_semantic, - ompl_planning_pipeline_config, - kinematics_yaml]) + moveit_config.move_group_capabilities = {"capabilities": ""} + + # Move group + ld.add_entity(generate_move_group_launch(moveit_config)) + # RViz + ld.add_entity(generate_moveit_rviz_launch(moveit_config)) + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + ld.add_action( + DeclareLaunchArgument( + "rviz_config", + default_value=get_package_share_directory("crane_plus_moveit_config") + + "/config/moveit.rviz", + description="Set the path to rviz configuration file.", + ) + ) + # Static TF - static_tf = Node(package='tf2_ros', - executable='static_transform_publisher', - name='static_transform_publisher', - output='log', - arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'base_link']) + ld.add_action( + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], + ) + ) # Publish TF - robot_state_publisher = Node(package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[robot_description]) - - return LaunchDescription([declare_loaded_description, - declare_rviz_config_file, - run_move_group_node, - rviz_node, - static_tf, - robot_state_publisher, - ]) + ld.add_entity(generate_rsp_launch(moveit_config)) + + return ld \ No newline at end of file diff --git a/crane_plus_moveit_config/launch/run_move_group.rviz b/crane_plus_moveit_config/launch/run_move_group.rviz old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 00000000..e16485a5 --- /dev/null +++ b/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/spawn_controllers.launch.py b/crane_plus_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 00000000..85a8dfc7 --- /dev/null +++ b/crane_plus_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py b/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 00000000..7ff90d29 --- /dev/null +++ b/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/warehouse_db.launch.py b/crane_plus_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 00000000..92981372 --- /dev/null +++ b/crane_plus_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/crane_plus_moveit_config/package.xml b/crane_plus_moveit_config/package.xml old mode 100644 new mode 100755 index 857a4e80..108dd2cf --- a/crane_plus_moveit_config/package.xml +++ b/crane_plus_moveit_config/package.xml @@ -2,26 +2,50 @@ crane_plus_moveit_config - 2.0.1 - CRANE+ V2 move_group config package + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the crane_plus with the MoveIt Motion Planning Framework + RT Corporation - Apache License 2.0 - Shota Aoki - Atsushi Kuwagata + BSD-3-Clause + + http://moveit.ros.org/ + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 + + RT Corporation ament_cmake - moveit - robot_state_publisher - rviz2 - tf2_ros - xacro + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + crane_plus_description + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo - ament_lint_auto - ament_lint_common - ament_cmake + ament_cmake diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py new file mode 100644 index 00000000..3c6d9e87 --- /dev/null +++ b/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py new file mode 100644 index 00000000..f67eaa98 --- /dev/null +++ b/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config new file mode 100644 index 00000000..72c9aca4 --- /dev/null +++ b/install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config @@ -0,0 +1 @@ +moveit_ros_move_group;moveit_kinematics;moveit_planners;moveit_simple_controller_manager;joint_state_publisher;joint_state_publisher_gui;tf2_ros;xacro;controller_manager;crane_plus_description;moveit_configs_utils;moveit_ros_visualization;moveit_ros_warehouse;moveit_setup_assistant;robot_state_publisher;rviz2;rviz_common;rviz_default_plugins;warehouse_ros_mongo \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/ament_index/resource_index/packages/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/ament_index/resource_index/packages/crane_plus_moveit_config new file mode 100644 index 00000000..e69de29b diff --git a/install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config new file mode 100644 index 00000000..55163222 --- /dev/null +++ b/install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config @@ -0,0 +1 @@ +/home/user/overlay_ws/install/crane_plus_moveit_config_new:/home/user/overlay_ws/install/crane_plus:/home/user/overlay_ws/install/crane_plus_gazebo:/home/user/overlay_ws/install/crane_plus_examples:/home/user/overlay_ws/install/crane_plus_moveit_config:/home/user/overlay_ws/install/crane_plus_control:/home/user/overlay_ws/install/crane_plus_description:/opt/ros/jazzy \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config new file mode 100644 index 00000000..c3a13fbd --- /dev/null +++ b/install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config @@ -0,0 +1 @@ +controller_manager:crane_plus_description:joint_state_publisher:joint_state_publisher_gui:moveit_configs_utils:moveit_kinematics:moveit_planners:moveit_ros_move_group:moveit_ros_visualization:moveit_ros_warehouse:moveit_setup_assistant:moveit_simple_controller_manager:robot_state_publisher:rviz2:rviz_common:rviz_default_plugins:tf2_ros:warehouse_ros_mongo:xacro \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant new file mode 100644 index 00000000..b7b0cc0e --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant @@ -0,0 +1,10 @@ +moveit_setup_assistant_config: + urdf: + package: crane_plus_description + relative_path: urdf/crane_plus.urdf.xacro + srdf: + relative_path: config/crane_plus.srdf + package_settings: + author_name: RT Corporation + author_email: shop@rt-net.jp + generated_timestamp: 1721806216 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake new file mode 100644 index 00000000..96789497 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake @@ -0,0 +1,14 @@ +# generated from ament/cmake/core/templates/nameConfig-version.cmake.in +set(PACKAGE_VERSION "0.3.0") + +set(PACKAGE_VERSION_EXACT False) +set(PACKAGE_VERSION_COMPATIBLE False) + +if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") + set(PACKAGE_VERSION_EXACT True) + set(PACKAGE_VERSION_COMPATIBLE True) +endif() + +if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") + set(PACKAGE_VERSION_COMPATIBLE True) +endif() diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake new file mode 100644 index 00000000..d79b9053 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake @@ -0,0 +1,42 @@ +# generated from ament/cmake/core/templates/nameConfig.cmake.in + +# prevent multiple inclusion +if(_crane_plus_moveit_config_CONFIG_INCLUDED) + # ensure to keep the found flag the same + if(NOT DEFINED crane_plus_moveit_config_FOUND) + # explicitly set it to FALSE, otherwise CMake will set it to TRUE + set(crane_plus_moveit_config_FOUND FALSE) + elseif(NOT crane_plus_moveit_config_FOUND) + # use separate condition to avoid uninitialized variable warning + set(crane_plus_moveit_config_FOUND FALSE) + endif() + return() +endif() +set(_crane_plus_moveit_config_CONFIG_INCLUDED TRUE) + +# output package information +if(NOT crane_plus_moveit_config_FIND_QUIETLY) + message(STATUS "Found crane_plus_moveit_config: 0.3.0 (${crane_plus_moveit_config_DIR})") +endif() + +# warn when using a deprecated package +if(NOT "" STREQUAL "") + set(_msg "Package 'crane_plus_moveit_config' is deprecated") + # append custom deprecation text if available + if(NOT "" STREQUAL "TRUE") + set(_msg "${_msg} ()") + endif() + # optionally quiet the deprecation message + if(NOT crane_plus_moveit_config_DEPRECATED_QUIET) + message(DEPRECATION "${_msg}") + endif() +endif() + +# flag package as ament-based to distinguish it after being find_package()-ed +set(crane_plus_moveit_config_FOUND_AMENT_PACKAGE TRUE) + +# include all config extra files +set(_extras "") +foreach(_extra ${_extras}) + include("${crane_plus_moveit_config_DIR}/${_extra}") +endforeach() diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml new file mode 100644 index 00000000..e108e7b3 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml @@ -0,0 +1,27 @@ +trajectory_execution: + - allowed_execution_duration_scaling: 1.2 + - allowed_goal_duration_margin: 0.5 + - allowed_start_tolerance: 0.1 + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +controller_names: + - crane_plus_arm_controller + - crane_plus_gripper_controller + +crane_plus_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - crane_plus_joint1 + - crane_plus_joint2 + - crane_plus_joint3 + - crane_plus_joint4 + +crane_plus_gripper_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - crane_plus_joint_hand diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf new file mode 100644 index 00000000..af40281f --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml new file mode 100644 index 00000000..bd4b95ae --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml @@ -0,0 +1,35 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + crane_plus_joint1: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + crane_plus_joint2: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + crane_plus_joint3: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + crane_plus_joint4: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + crane_plus_joint_hand: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml new file mode 100644 index 00000000..193fbb96 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +arm: + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz new file mode 100644 index 00000000..f31651ed --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml new file mode 100644 index 00000000..f32aafdf --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,196 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. +request_adapters: + # - default_planning_request_adapters/ResolveConstraintFrames + # - default_planning_request_adapters/ValidateWorkspaceBounds + # - default_planning_request_adapters/CheckStartStateBounds + # - default_planning_request_adapters/CheckStartStateCollision + - default_planning_request_adapters/AddTimeOptimalParameterization + - default_planning_request_adapters/FixWorkspaceBounds + - default_planning_request_adapters/FixStartStateBounds + - default_planning_request_adapters/FixStartStatePathConstraints +response_adapters: + # - default_planning_response_adapters/AddTimeOptimalParameterization + # - default_planning_response_adapters/ValidateSolution + # - default_planning_response_adapters/DisplayMotionPath + +start_state_max_bounds_error: 0.1 + +planner_configs: + 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 + +arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +gripper: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml new file mode 100644 index 00000000..5d90e51d --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,23 @@ +sensors: + - kinect_pointcloud + - kinect_depthimage +kinect_pointcloud: + filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater +kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv new file mode 100644 index 00000000..79d4c95b --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv @@ -0,0 +1 @@ +prepend-non-duplicate;AMENT_PREFIX_PATH; diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh new file mode 100644 index 00000000..02e441b7 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh @@ -0,0 +1,4 @@ +# copied from +# ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh + +ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX" diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv new file mode 100644 index 00000000..b94426af --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv @@ -0,0 +1 @@ +prepend-non-duplicate-if-exists;PATH;bin diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh new file mode 100644 index 00000000..e59b749a --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh @@ -0,0 +1,5 @@ +# copied from ament_cmake_core/cmake/environment_hooks/environment/path.sh + +if [ -d "$AMENT_CURRENT_PREFIX/bin" ]; then + ament_prepend_unique_value PATH "$AMENT_CURRENT_PREFIX/bin" +fi diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv new file mode 100644 index 00000000..e119f32c --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv @@ -0,0 +1 @@ +prepend-non-duplicate;CMAKE_PREFIX_PATH; diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 new file mode 100644 index 00000000..d03facc1 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 @@ -0,0 +1,3 @@ +# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em + +colcon_prepend_unique_value CMAKE_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX" diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh new file mode 100644 index 00000000..a948e685 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh @@ -0,0 +1,3 @@ +# generated from colcon_core/shell/template/hook_prepend_value.sh.em + +_colcon_prepend_unique_value CMAKE_PREFIX_PATH "$COLCON_CURRENT_PREFIX" diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py new file mode 100644 index 00000000..2b134ee0 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py new file mode 100644 index 00000000..d05ee6c5 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 00000000..9034635d --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py new file mode 100644 index 00000000..28439b32 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py new file mode 100644 index 00000000..398f8808 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -0,0 +1,237 @@ +# Copyright 2020 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 +import yaml +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 + +# Reference: https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes/ +# run_move_group/launch/run_move_group.launch.py + + +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(): + declare_loaded_description = DeclareLaunchArgument( + "loaded_description", + default_value="", + description="Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + ) + + declare_rviz_config_file = DeclareLaunchArgument( + "rviz_config_file", + default_value=get_package_share_directory("crane_plus_moveit_config") + + "/launch/run_move_group.rviz", + description="Set the path to rviz configuration file.", + ) + + # robot_description = {"robot_description": LaunchConfiguration("loaded_description")} + + # robot_description_semantic_config = load_file( + # "crane_plus_moveit_config", "config/crane_plus.srdf" + # ) + # robot_description_semantic = { + # "robot_description_semantic": robot_description_semantic_config + # } + + # kinematics_yaml = load_yaml("crane_plus_moveit_config", "config/kinematics.yaml") + + # Planning Functionality + # ompl_planning_pipeline_config = {'move_group': { + # 'planning_plugin': 'ompl_interface/OMPLPlanner', + # 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization \ + # default_planner_request_adapters/FixWorkspaceBounds \ + # default_planner_request_adapters/FixStartStateBounds \ + # default_planner_request_adapters/FixStartStateCollision \ + # default_planner_request_adapters/FixStartStatePathConstraints', + # 'start_state_max_bounds_error': 0.1}} + # ompl_planning_yaml = load_yaml('crane_plus_moveit_config', 'config/ompl_planning.yaml') + # ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + # controllers_yaml = load_yaml('crane_plus_moveit_config', 'config/controllers.yaml') + # moveit_controllers = { + # 'moveit_simple_controller_manager': controllers_yaml, + # 'moveit_controller_manager': + # 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} + + # trajectory_execution= {'moveit_manage_controllers': True, + # 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + # 'trajectory_execution.allowed_goal_duration_margin': 0.5, + # 'trajectory_execution.allowed_start_tolerance': 0.1} + + # planning_scene_monitor_parameters = {'publish_planning_scene': True, + # 'publish_geometry_updates': True, + # 'publish_state_updates': True, + # 'publish_transforms_updates': True} + + + # MoveItConfigBuilderによるパラメータ設定 + moveit_config = ( + # ロボット名の定義 + MoveItConfigsBuilder("crane_plus") + # URDFの設定 + .robot_description( + mappings={"robot_description": LaunchConfiguration("loaded_description")} + ) + # SRDFの設定 + .robot_description_semantic(file_path="config/crane_plus.srdf") + # キネマティクスの設定 + .robot_description_kinematics(file_path="config/kinematics.yaml") + # 軌道計画のプラグイン設定 + .planning_pipelines( + pipelines=["ompl"], + default_planning_pipeline="ompl", + ) + # 軌道追従制御ノードの設定 + .trajectory_execution( + # file_path="controllers.yaml", + file_path="config/controllers.yaml", + moveit_manage_controllers=True, + ) + .joint_limits( + file_path="config/joint_limits.yaml" + ) + # Planning Sceneのトピックの設定 + .planning_scene_monitor( + publish_planning_scene=True, + publish_geometry_updates=True, + publish_state_updates=True, + publish_transforms_updates=True, + ).to_moveit_configs() + ) + + # Start the actual move_group node/action server + # run_move_group_node = Node(package='moveit_ros_move_group', + # executable='move_group', + # output='screen', + # parameters=[robot_description, + # robot_description_semantic, + # kinematics_yaml, + # ompl_planning_pipeline_config, + # trajectory_execution, + # moveit_controllers, + # planning_scene_monitor_parameters]) + + # Start the actual move_group node/action server + # パラメータをMoveItConfigsBuilderを使った渡し方に変更 + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + arguments=["--ros-args", "--log-level", "info"], + ) + + # RViz + rviz_config_file = LaunchConfiguration("rviz_config_file") + # rviz_node = Node(package='rviz2', + # executable='rviz2', + # name='rviz2', + # output='log', + # arguments=['-d', rviz_config_file], + # parameters=[robot_description, + # robot_description_semantic, + # ompl_planning_pipeline_config, + # kinematics_yaml]) + + # MoveitConfigsBuilderのparameter指定に書き直す + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + moveit_config.joint_limits, + ], + ) + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], + ) + + # Publish TF + # MoveitConfigsBuilderのparameter指定に書き直す + # robot_state_publisher = Node( + # package="robot_state_publisher", + # executable="robot_state_publisher", + # name="robot_state_publisher", + # output="both", + # parameters=[robot_description], + # ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + return LaunchDescription( + [ + declare_loaded_description, + declare_rviz_config_file, + run_move_group_node, + rviz_node, + static_tf, + robot_state_publisher, + ] + ) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz new file mode 100644 index 00000000..44b5c6a9 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz @@ -0,0 +1,351 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /MotionPlanning1 + - /Trajectory1 + Splitter Ratio: 0.5 + Tree Height: 295 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + crane_plus_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + crane_plus_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + crane_plus_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + crane_plus_link_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: display_planned_path + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.8449440598487854 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.04136588051915169 + Y: -0.017012163996696472 + Z: 0.08837244659662247 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.44539815187454224 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.5553978681564331 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0 diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 00000000..e16485a5 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 00000000..85a8dfc7 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 00000000..7ff90d29 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 00000000..92981372 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash new file mode 100644 index 00000000..49782f24 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash @@ -0,0 +1,46 @@ +# generated from ament_package/template/package_level/local_setup.bash.in + +# source local_setup.sh from same directory as this file +_this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd) +# provide AMENT_CURRENT_PREFIX to shell script +AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd) +# store AMENT_CURRENT_PREFIX to restore it before each environment hook +_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX + +# trace output +if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then + echo "# . \"$_this_path/local_setup.sh\"" +fi +. "$_this_path/local_setup.sh" +unset _this_path + +# unset AMENT_ENVIRONMENT_HOOKS +# if not appending to them for return +if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then + unset AMENT_ENVIRONMENT_HOOKS +fi + +# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks +AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX +# list all environment hooks of this package + +# source all shell-specific environment hooks of this package +# if not returning them +if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then + _package_local_setup_IFS=$IFS + IFS=":" + for _hook in $AMENT_ENVIRONMENT_HOOKS; do + # restore AMENT_CURRENT_PREFIX for each environment hook + AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX + # restore IFS before sourcing other files + IFS=$_package_local_setup_IFS + . "$_hook" + done + unset _hook + IFS=$_package_local_setup_IFS + unset _package_local_setup_IFS + unset AMENT_ENVIRONMENT_HOOKS +fi + +unset _package_local_setup_AMENT_CURRENT_PREFIX +unset AMENT_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv new file mode 100644 index 00000000..a5047579 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv @@ -0,0 +1,2 @@ +source;share/crane_plus_moveit_config/environment/ament_prefix_path.sh +source;share/crane_plus_moveit_config/environment/path.sh diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh new file mode 100644 index 00000000..4f258b12 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh @@ -0,0 +1,184 @@ +# generated from ament_package/template/package_level/local_setup.sh.in + +# since this file is sourced use either the provided AMENT_CURRENT_PREFIX +# or fall back to the destination set at configure time +: ${AMENT_CURRENT_PREFIX:="/home/user/overlay_ws/src/crane_plus/install/crane_plus_moveit_config"} +if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then + if [ -z "$COLCON_CURRENT_PREFIX" ]; then + echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \ + "exist. Consider sourcing a different extension than '.sh'." 1>&2 + else + AMENT_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" + fi +fi + +# function to append values to environment variables +# using colons as separators and avoiding leading separators +ament_append_value() { + # arguments + _listname="$1" + _value="$2" + #echo "listname $_listname" + #eval echo "list value \$$_listname" + #echo "value $_value" + + # avoid leading separator + eval _values=\"\$$_listname\" + if [ -z "$_values" ]; then + eval export $_listname=\"$_value\" + #eval echo "set list \$$_listname" + else + # field separator must not be a colon + _ament_append_value_IFS=$IFS + unset IFS + eval export $_listname=\"\$$_listname:$_value\" + #eval echo "append list \$$_listname" + IFS=$_ament_append_value_IFS + unset _ament_append_value_IFS + fi + unset _values + + unset _value + unset _listname +} + +# function to append non-duplicate values to environment variables +# using colons as separators and avoiding leading separators +ament_append_unique_value() { + # arguments + _listname=$1 + _value=$2 + #echo "listname $_listname" + #eval echo "list value \$$_listname" + #echo "value $_value" + + # check if the list contains the value + eval _values=\$$_listname + _duplicate= + _ament_append_unique_value_IFS=$IFS + IFS=":" + if [ "$AMENT_SHELL" = "zsh" ]; then + ament_zsh_to_array _values + fi + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + if [ $_item = $_value ]; then + _duplicate=1 + fi + done + unset _item + + # append only non-duplicates + if [ -z "$_duplicate" ]; then + # avoid leading separator + if [ -z "$_values" ]; then + eval $_listname=\"$_value\" + #eval echo "set list \$$_listname" + else + # field separator must not be a colon + unset IFS + eval $_listname=\"\$$_listname:$_value\" + #eval echo "append list \$$_listname" + fi + fi + IFS=$_ament_append_unique_value_IFS + unset _ament_append_unique_value_IFS + unset _duplicate + unset _values + + unset _value + unset _listname +} + +# function to prepend non-duplicate values to environment variables +# using colons as separators and avoiding trailing separators +ament_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + #echo "listname $_listname" + #eval echo "list value \$$_listname" + #echo "value $_value" + + # check if the list contains the value + eval _values=\"\$$_listname\" + _duplicate= + _ament_prepend_unique_value_IFS=$IFS + IFS=":" + if [ "$AMENT_SHELL" = "zsh" ]; then + ament_zsh_to_array _values + fi + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + if [ "$_item" = "$_value" ]; then + _duplicate=1 + fi + done + unset _item + + # prepend only non-duplicates + if [ -z "$_duplicate" ]; then + # avoid trailing separator + if [ -z "$_values" ]; then + eval export $_listname=\"$_value\" + #eval echo "set list \$$_listname" + else + # field separator must not be a colon + unset IFS + eval export $_listname=\"$_value:\$$_listname\" + #eval echo "prepend list \$$_listname" + fi + fi + IFS=$_ament_prepend_unique_value_IFS + unset _ament_prepend_unique_value_IFS + unset _duplicate + unset _values + + unset _value + unset _listname +} + +# unset AMENT_ENVIRONMENT_HOOKS +# if not appending to them for return +if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then + unset AMENT_ENVIRONMENT_HOOKS +fi + +# list all environment hooks of this package +ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/crane_plus_moveit_config/environment/ament_prefix_path.sh" +ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/crane_plus_moveit_config/environment/path.sh" + +# source all shell-specific environment hooks of this package +# if not returning them +if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then + _package_local_setup_IFS=$IFS + IFS=":" + if [ "$AMENT_SHELL" = "zsh" ]; then + ament_zsh_to_array AMENT_ENVIRONMENT_HOOKS + fi + for _hook in $AMENT_ENVIRONMENT_HOOKS; do + if [ -f "$_hook" ]; then + # restore IFS before sourcing other files + IFS=$_package_local_setup_IFS + # trace output + if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then + echo "# . \"$_hook\"" + fi + . "$_hook" + fi + done + unset _hook + IFS=$_package_local_setup_IFS + unset _package_local_setup_IFS + unset AMENT_ENVIRONMENT_HOOKS +fi + +# reset AMENT_CURRENT_PREFIX after each package +# allowing to source multiple package-level setup files +unset AMENT_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh new file mode 100644 index 00000000..fe161be5 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh @@ -0,0 +1,59 @@ +# generated from ament_package/template/package_level/local_setup.zsh.in + +AMENT_SHELL=zsh + +# source local_setup.sh from same directory as this file +_this_path=$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd) +# provide AMENT_CURRENT_PREFIX to shell script +AMENT_CURRENT_PREFIX=$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd) +# store AMENT_CURRENT_PREFIX to restore it before each environment hook +_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX + +# function to convert array-like strings into arrays +# to wordaround SH_WORD_SPLIT not being set +ament_zsh_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# trace output +if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then + echo "# . \"$_this_path/local_setup.sh\"" +fi +# the package-level local_setup file unsets AMENT_CURRENT_PREFIX +. "$_this_path/local_setup.sh" +unset _this_path + +# unset AMENT_ENVIRONMENT_HOOKS +# if not appending to them for return +if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then + unset AMENT_ENVIRONMENT_HOOKS +fi + +# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks +AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX +# list all environment hooks of this package + +# source all shell-specific environment hooks of this package +# if not returning them +if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then + _package_local_setup_IFS=$IFS + IFS=":" + for _hook in $AMENT_ENVIRONMENT_HOOKS; do + # restore AMENT_CURRENT_PREFIX for each environment hook + AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX + # restore IFS before sourcing other files + IFS=$_package_local_setup_IFS + . "$_hook" + done + unset _hook + IFS=$_package_local_setup_IFS + unset _package_local_setup_IFS + unset AMENT_ENVIRONMENT_HOOKS +fi + +unset _package_local_setup_AMENT_CURRENT_PREFIX +unset AMENT_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash new file mode 100644 index 00000000..27a9ab68 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash @@ -0,0 +1,39 @@ +# generated from colcon_bash/shell/template/package.bash.em + +# This script extends the environment for this package. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + # the prefix is two levels up from the package specific share directory + _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)" +else + _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +_colcon_package_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$@" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source sh script of this package +_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/package.sh" + +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts +COLCON_CURRENT_PREFIX="$_colcon_package_bash_COLCON_CURRENT_PREFIX" + +# source bash hooks +_colcon_package_bash_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/local_setup.bash" + +unset COLCON_CURRENT_PREFIX + +unset _colcon_package_bash_source_script +unset _colcon_package_bash_COLCON_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv new file mode 100644 index 00000000..bf76ceda --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv @@ -0,0 +1,8 @@ +source;share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 +source;share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv +source;share/crane_plus_moveit_config/hook/cmake_prefix_path.sh +source;share/crane_plus_moveit_config/local_setup.bash +source;share/crane_plus_moveit_config/local_setup.dsv +source;share/crane_plus_moveit_config/local_setup.ps1 +source;share/crane_plus_moveit_config/local_setup.sh +source;share/crane_plus_moveit_config/local_setup.zsh diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 new file mode 100644 index 00000000..6e321cf1 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 @@ -0,0 +1,116 @@ +# generated from colcon_powershell/shell/template/package.ps1.em + +# function to append a value to a variable +# which uses colons as separators +# duplicates as well as leading separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +function colcon_append_unique_value { + param ( + $_listname, + $_value + ) + + # get values from variable + if (Test-Path Env:$_listname) { + $_values=(Get-Item env:$_listname).Value + } else { + $_values="" + } + $_duplicate="" + # start with no values + $_all_values="" + # iterate over existing values in the variable + if ($_values) { + $_values.Split(";") | ForEach { + # not an empty string + if ($_) { + # not a duplicate of _value + if ($_ -eq $_value) { + $_duplicate="1" + } + if ($_all_values) { + $_all_values="${_all_values};$_" + } else { + $_all_values="$_" + } + } + } + } + # append only non-duplicates + if (!$_duplicate) { + # avoid leading separator + if ($_all_values) { + $_all_values="${_all_values};${_value}" + } else { + $_all_values="${_value}" + } + } + + # export the updated variable + Set-Item env:\$_listname -Value "$_all_values" +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +function colcon_prepend_unique_value { + param ( + $_listname, + $_value + ) + + # get values from variable + if (Test-Path Env:$_listname) { + $_values=(Get-Item env:$_listname).Value + } else { + $_values="" + } + # start with the new value + $_all_values="$_value" + # iterate over existing values in the variable + if ($_values) { + $_values.Split(";") | ForEach { + # not an empty string + if ($_) { + # not a duplicate of _value + if ($_ -ne $_value) { + # keep non-duplicate values + $_all_values="${_all_values};$_" + } + } + } + } + # export the updated variable + Set-Item env:\$_listname -Value "$_all_values" +} + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +function colcon_package_source_powershell_script { + param ( + $_colcon_package_source_powershell_script + ) + # source script with conditional trace output + if (Test-Path $_colcon_package_source_powershell_script) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_package_source_powershell_script'" + } + . "$_colcon_package_source_powershell_script" + } else { + Write-Error "not found: '$_colcon_package_source_powershell_script'" + } +} + + +# a powershell script is able to determine its own path +# the prefix is two levels up from the package specific share directory +$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName + +colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1" +colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/crane_plus_moveit_config/local_setup.ps1" + +Remove-Item Env:\COLCON_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh new file mode 100644 index 00000000..57bb4350 --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh @@ -0,0 +1,87 @@ +# generated from colcon_core/shell/template/package.sh.em + +# This script extends the environment for this package. + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prepend_unique_value_IFS=$IFS + IFS=":" + # start with the new value + _all_values="$_value" + # workaround SH_WORD_SPLIT not being set in zsh + if [ "$(command -v colcon_zsh_convert_to_array)" ]; then + colcon_zsh_convert_to_array _values + fi + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + # restore the field separator + IFS=$_colcon_prepend_unique_value_IFS + unset _colcon_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/user/overlay_ws/src/crane_plus/install/crane_plus_moveit_config" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_package_sh_COLCON_CURRENT_PREFIX + return 1 + fi + COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX" +fi +unset _colcon_package_sh_COLCON_CURRENT_PREFIX + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +_colcon_package_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$@" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source sh hooks +_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh" +_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/local_setup.sh" + +unset _colcon_package_sh_source_script +unset COLCON_CURRENT_PREFIX + +# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml new file mode 100644 index 00000000..108dd2cf --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml @@ -0,0 +1,51 @@ + + + + crane_plus_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the crane_plus with the MoveIt Motion Planning Framework + + RT Corporation + + BSD-3-Clause + + http://moveit.ros.org/ + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 + + RT Corporation + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + crane_plus_description + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + + + + ament_cmake + + diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh new file mode 100644 index 00000000..8e6006dc --- /dev/null +++ b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh @@ -0,0 +1,50 @@ +# generated from colcon_zsh/shell/template/package.zsh.em + +# This script extends the environment for this package. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + # the prefix is two levels up from the package specific share directory + _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)" +else + _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +# additional arguments: arguments to the script +_colcon_package_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$@" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +colcon_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# source sh script of this package +_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/package.sh" +unset convert_zsh_to_array + +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts +COLCON_CURRENT_PREFIX="$_colcon_package_zsh_COLCON_CURRENT_PREFIX" + +# source zsh hooks +_colcon_package_zsh_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX + +unset _colcon_package_zsh_source_script +unset _colcon_package_zsh_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.bash b/install/local_setup.bash new file mode 100644 index 00000000..03f00256 --- /dev/null +++ b/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.ps1 b/install/local_setup.ps1 new file mode 100644 index 00000000..6f68c8de --- /dev/null +++ b/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/install/local_setup.sh b/install/local_setup.sh new file mode 100644 index 00000000..78e2b408 --- /dev/null +++ b/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/user/overlay_ws/src/crane_plus/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.zsh b/install/local_setup.zsh new file mode 100644 index 00000000..b6487102 --- /dev/null +++ b/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/install/setup.bash b/install/setup.bash new file mode 100644 index 00000000..07a556ce --- /dev/null +++ b/install/setup.bash @@ -0,0 +1,34 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/jazzy" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/overlay_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.ps1 b/install/setup.ps1 new file mode 100644 index 00000000..2d91df92 --- /dev/null +++ b/install/setup.ps1 @@ -0,0 +1,30 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/jazzy\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/user/overlay_ws/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/install/setup.sh b/install/setup.sh new file mode 100644 index 00000000..78792771 --- /dev/null +++ b/install/setup.sh @@ -0,0 +1,49 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/user/overlay_ws/src/crane_plus/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/jazzy" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/user/overlay_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/install/setup.zsh b/install/setup.zsh new file mode 100644 index 00000000..99962a2a --- /dev/null +++ b/install/setup.zsh @@ -0,0 +1,34 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/jazzy" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/overlay_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 00000000..e69de29b diff --git a/log/latest b/log/latest new file mode 120000 index 00000000..b57d247c --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/log/latest_build b/log/latest_build new file mode 120000 index 00000000..2552eddc --- /dev/null +++ b/log/latest_build @@ -0,0 +1 @@ +build_2024-07-24_16-33-58 \ No newline at end of file From 1374d50e84a6fa3f5af662a36da6db3a159ee12a Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 9 Aug 2024 11:48:37 +0900 Subject: [PATCH 002/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=82=B3?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_description/package.xml | 2 -- crane_plus_examples/launch/demo.launch.py | 2 -- crane_plus_examples/src/aruco_detection.cpp | 1 - crane_plus_examples/src/color_detection.cpp | 2 -- crane_plus_moveit_config/.setup_assistant | 25 +++++++++++++++++++ .../config/joint_limits.yaml | 8 ++---- .../config/kinematics.yaml | 3 ++- .../config/sensors_3d.yaml | 3 ++- .../launch/run_move_group.launch.py | 2 +- crane_plus_moveit_config/package.xml | 4 --- .../crane_plus_moveit_config | 1 - 11 files changed, 32 insertions(+), 21 deletions(-) create mode 100755 crane_plus_moveit_config/.setup_assistant delete mode 100644 install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config diff --git a/crane_plus_description/package.xml b/crane_plus_description/package.xml index e8805be6..1e5ff7ec 100644 --- a/crane_plus_description/package.xml +++ b/crane_plus_description/package.xml @@ -13,8 +13,6 @@ ament_cmake ament_cmake_python - gz_ros2_control joint_state_publisher_gui launch diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 16e3bd93..49e90d08 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -96,7 +96,5 @@ def generate_launch_description(): declare_use_camera, declare_video_device, move_group, - # move_group_camera, control_node, - # usb_cam_node ]) diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index 926726a6..7a70f071 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -27,7 +27,6 @@ #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" #include "opencv2/aruco.hpp" -// #include "cv_bridge/cv_bridge.h" #include "cv_bridge/cv_bridge.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Matrix3x3.h" diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp index 5e274136..73d98682 100644 --- a/crane_plus_examples/src/color_detection.cpp +++ b/crane_plus_examples/src/color_detection.cpp @@ -29,9 +29,7 @@ #include "tf2_ros/transform_broadcaster.h" #include "opencv2/opencv.hpp" #include "opencv2/imgproc/imgproc.hpp" -// #include "cv_bridge/cv_bridge.h" #include "cv_bridge/cv_bridge.hpp" -// #include "image_geometry/pinhole_camera_model.h" #include "image_geometry/pinhole_camera_model.hpp" using std::placeholders::_1; diff --git a/crane_plus_moveit_config/.setup_assistant b/crane_plus_moveit_config/.setup_assistant new file mode 100755 index 00000000..3058c8a2 --- /dev/null +++ b/crane_plus_moveit_config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: crane_plus_description + relative_path: urdf/crane_plus.urdf.xacro + srdf: + relative_path: config/crane_plus.srdf + package_settings: + author_name: RT Corporation + author_email: shop@rt-net.jp + generated_timestamp: 1721799427 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/crane_plus_moveit_config/config/joint_limits.yaml b/crane_plus_moveit_config/config/joint_limits.yaml index ac0df2e2..45544801 100755 --- a/crane_plus_moveit_config/config/joint_limits.yaml +++ b/crane_plus_moveit_config/config/joint_limits.yaml @@ -11,30 +11,26 @@ joint_limits: crane_plus_joint1: has_velocity_limits: true max_velocity: 2.0 - # has_acceleration_limits: false has_acceleration_limits: true max_acceleration: 1.0 crane_plus_joint2: has_velocity_limits: true max_velocity: 2.0 - # has_acceleration_limits: false has_acceleration_limits: true max_acceleration: 1.0 crane_plus_joint3: has_velocity_limits: true max_velocity: 2.0 - # has_acceleration_limits: false has_acceleration_limits: true max_acceleration: 1.0 crane_plus_joint4: has_velocity_limits: true max_velocity: 2.0 - # has_acceleration_limits: false has_acceleration_limits: true max_acceleration: 1.0 crane_plus_joint_hand: has_velocity_limits: true max_velocity: 2.0 - # has_acceleration_limits: false has_acceleration_limits: true - max_acceleration: 1.0 \ No newline at end of file + max_acceleration: 1.0 + \ No newline at end of file diff --git a/crane_plus_moveit_config/config/kinematics.yaml b/crane_plus_moveit_config/config/kinematics.yaml index a1f17d08..c9fd73d3 100755 --- a/crane_plus_moveit_config/config/kinematics.yaml +++ b/crane_plus_moveit_config/config/kinematics.yaml @@ -2,4 +2,5 @@ arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 \ No newline at end of file + kinematics_solver_attempts: 3 + \ No newline at end of file diff --git a/crane_plus_moveit_config/config/sensors_3d.yaml b/crane_plus_moveit_config/config/sensors_3d.yaml index 5d90e51d..ab5c1e18 100644 --- a/crane_plus_moveit_config/config/sensors_3d.yaml +++ b/crane_plus_moveit_config/config/sensors_3d.yaml @@ -20,4 +20,5 @@ kinect_depthimage: padding_scale: 4.0 queue_size: 5 sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - shadow_threshold: 0.2 \ No newline at end of file + shadow_threshold: 0.2 + \ No newline at end of file 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 bea9340b..fb4e9a8d 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -152,4 +152,4 @@ def generate_launch_description(): # Publish TF ld.add_entity(generate_rsp_launch(moveit_config)) - return ld \ No newline at end of file + return ld diff --git a/crane_plus_moveit_config/package.xml b/crane_plus_moveit_config/package.xml index 108dd2cf..d4e67418 100755 --- a/crane_plus_moveit_config/package.xml +++ b/crane_plus_moveit_config/package.xml @@ -26,10 +26,6 @@ joint_state_publisher_gui tf2_ros xacro - - - controller_manager crane_plus_description moveit_configs_utils diff --git a/install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config deleted file mode 100644 index 72c9aca4..00000000 --- a/install/crane_plus_moveit_config/share/ament_index/resource_index/package_run_dependencies/crane_plus_moveit_config +++ /dev/null @@ -1 +0,0 @@ -moveit_ros_move_group;moveit_kinematics;moveit_planners;moveit_simple_controller_manager;joint_state_publisher;joint_state_publisher_gui;tf2_ros;xacro;controller_manager;crane_plus_description;moveit_configs_utils;moveit_ros_visualization;moveit_ros_warehouse;moveit_setup_assistant;robot_state_publisher;rviz2;rviz_common;rviz_default_plugins;warehouse_ros_mongo \ No newline at end of file From 6b10a749600b01235c5d5554b645155a9568b667 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Tue, 13 Aug 2024 10:53:39 +0900 Subject: [PATCH 003/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=83=95=E3=82=A1?= =?UTF-8?q?=E3=82=A4=E3=83=AB=E3=81=AEignore?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .docker/README.md | 0 .docker/source/Dockerfile | 0 .dockerignore | 0 .github/workflows/build_docker_image.yml | 0 .github/workflows/industrial_ci.yml | 0 .gitignore | 6 + LICENSE | 0 README.md | 0 crane_plus/CHANGELOG.rst | 0 crane_plus/CMakeLists.txt | 0 crane_plus/package.xml | 0 crane_plus_control/CHANGELOG.rst | 0 crane_plus_control/CMakeLists.txt | 0 crane_plus_control/README.md | 0 .../config/crane_plus_controllers.yaml | 0 crane_plus_control/crane_plus_hardware.xml | 0 .../crane_plus_control/crane_plus_driver.hpp | 0 .../crane_plus_hardware.hpp | 0 .../crane_plus_control/visibility_control.h | 0 .../launch/crane_plus_control.launch.py | 0 crane_plus_control/package.xml | 0 crane_plus_control/src/crane_plus_driver.cpp | 0 .../src/crane_plus_hardware.cpp | 0 crane_plus_description/CHANGELOG.rst | 0 crane_plus_description/CMakeLists.txt | 0 crane_plus_description/README.md | 0 .../crane_plus_description/__init__.py | 0 .../launch/display.launch.py | 0 crane_plus_description/launch/display.rviz | 0 .../collision/Hand_collision_20201013.stl | Bin .../collision/Link1_collision_20201013.stl | Bin .../collision/Link2_collision_20201013.stl | Bin .../collision/Link3_collision_20201013.stl | Bin .../collision/Link4_collision_20201013.stl | Bin .../collision/body_collision_20201013.stl | Bin .../meshes/visual/Hand_20201013.stl | Bin .../meshes/visual/Link1_20201013.stl | Bin .../meshes/visual/Link2_20201013.stl | Bin .../meshes/visual/Link3_20201013.stl | Bin .../meshes/visual/Link4_20201013.stl | Bin .../meshes/visual/body_20201013.stl | Bin crane_plus_description/package.xml | 0 .../test/dummy_controllers.yaml | 0 .../test/test_robot_description_loader.py | 0 crane_plus_description/urdf/camera.urdf.xacro | 0 .../urdf/camera_stand.urdf.xacro | 0 .../urdf/crane_plus.gazebo.xacro | 0 .../urdf/crane_plus.gazebo_ros2_control.xacro | 0 .../urdf/crane_plus.ros2_control.xacro | 0 .../urdf/crane_plus.urdf.xacro | 0 crane_plus_description/urdf/crane_plus.xacro | 0 crane_plus_examples/CHANGELOG.rst | 0 crane_plus_examples/CMakeLists.txt | 0 crane_plus_examples/README.md | 0 crane_plus_examples/aruco_markers.pdf | Bin crane_plus_examples/config/camera_info.yaml | 0 .../launch/camera_example.launch.py | 0 .../launch/camera_example.rviz | 0 crane_plus_examples/launch/demo.launch.py | 0 crane_plus_examples/launch/example.launch.py | 0 crane_plus_examples/package.xml | 0 crane_plus_examples/src/aruco_detection.cpp | 0 crane_plus_examples/src/color_detection.cpp | 0 crane_plus_examples/src/gripper_control.cpp | 0 crane_plus_examples/src/joint_values.cpp | 0 crane_plus_examples/src/pick_and_place.cpp | 0 crane_plus_examples/src/pick_and_place_tf.cpp | 0 crane_plus_examples/src/pose_groupstate.cpp | 0 crane_plus_gazebo/CHANGELOG.rst | 0 crane_plus_gazebo/CMakeLists.txt | 0 crane_plus_gazebo/README.md | 0 crane_plus_gazebo/gui/gui.config | 0 .../launch/crane_plus_with_table.launch.py | 0 crane_plus_gazebo/package.xml | 0 crane_plus_gazebo/worlds/table.sdf | 0 crane_plus_moveit_config/.setup_assistant | 25 -- crane_plus_moveit_config/config/moveit.rviz | 0 .../config/pilz_cartesian_limits.yaml | 0 .../config/sensors_3d.yaml | 0 .../launch/demo.launch.py | 0 .../launch/move_group.launch.py | 0 .../launch/moveit_rviz.launch.py | 0 crane_plus_moveit_config/launch/rsp.launch.py | 0 .../launch/setup_assistant.launch.py | 0 .../launch/spawn_controllers.launch.py | 0 .../launch/static_virtual_joint_tfs.launch.py | 0 .../launch/warehouse_db.launch.py | 0 install/COLCON_IGNORE | 0 install/_local_setup_util_ps1.py | 407 ------------------ install/_local_setup_util_sh.py | 407 ------------------ .../packages/crane_plus_moveit_config | 0 .../crane_plus_moveit_config | 1 - .../packages/crane_plus_moveit_config | 1 - .../crane_plus_moveit_config/.setup_assistant | 10 - ...ane_plus_moveit_configConfig-version.cmake | 14 - .../crane_plus_moveit_configConfig.cmake | 42 -- .../config/controllers.yaml | 27 -- .../config/crane_plus.srdf | 42 -- .../config/joint_limits.yaml | 35 -- .../config/kinematics.yaml | 4 - .../config/moveit.rviz | 51 --- .../config/ompl_planning.yaml | 196 --------- .../config/pilz_cartesian_limits.yaml | 6 - .../config/sensors_3d.yaml | 23 - .../environment/ament_prefix_path.dsv | 1 - .../environment/ament_prefix_path.sh | 4 - .../environment/path.dsv | 1 - .../environment/path.sh | 5 - .../hook/cmake_prefix_path.dsv | 1 - .../hook/cmake_prefix_path.ps1 | 3 - .../hook/cmake_prefix_path.sh | 3 - .../launch/demo.launch.py | 7 - .../launch/move_group.launch.py | 7 - .../launch/moveit_rviz.launch.py | 7 - .../launch/rsp.launch.py | 7 - .../launch/run_move_group.launch.py | 237 ---------- .../launch/run_move_group.rviz | 351 --------------- .../launch/setup_assistant.launch.py | 7 - .../launch/spawn_controllers.launch.py | 7 - .../launch/static_virtual_joint_tfs.launch.py | 7 - .../launch/warehouse_db.launch.py | 7 - .../crane_plus_moveit_config/local_setup.bash | 46 -- .../crane_plus_moveit_config/local_setup.dsv | 2 - .../crane_plus_moveit_config/local_setup.sh | 184 -------- .../crane_plus_moveit_config/local_setup.zsh | 59 --- .../crane_plus_moveit_config/package.bash | 39 -- .../crane_plus_moveit_config/package.dsv | 8 - .../crane_plus_moveit_config/package.ps1 | 116 ----- .../share/crane_plus_moveit_config/package.sh | 87 ---- .../crane_plus_moveit_config/package.xml | 51 --- .../crane_plus_moveit_config/package.zsh | 50 --- install/local_setup.bash | 121 ------ install/local_setup.ps1 | 55 --- install/local_setup.sh | 137 ------ install/local_setup.zsh | 134 ------ install/setup.bash | 34 -- install/setup.ps1 | 30 -- install/setup.sh | 49 --- install/setup.zsh | 34 -- log/COLCON_IGNORE | 0 log/latest | 1 - log/latest_build | 1 - 142 files changed, 6 insertions(+), 3191 deletions(-) mode change 100644 => 100755 .docker/README.md mode change 100644 => 100755 .docker/source/Dockerfile mode change 100644 => 100755 .dockerignore mode change 100644 => 100755 .github/workflows/build_docker_image.yml mode change 100644 => 100755 .github/workflows/industrial_ci.yml mode change 100644 => 100755 .gitignore mode change 100644 => 100755 LICENSE mode change 100644 => 100755 README.md mode change 100644 => 100755 crane_plus/CHANGELOG.rst mode change 100644 => 100755 crane_plus/CMakeLists.txt mode change 100644 => 100755 crane_plus/package.xml mode change 100644 => 100755 crane_plus_control/CHANGELOG.rst mode change 100644 => 100755 crane_plus_control/CMakeLists.txt mode change 100644 => 100755 crane_plus_control/README.md mode change 100644 => 100755 crane_plus_control/config/crane_plus_controllers.yaml mode change 100644 => 100755 crane_plus_control/crane_plus_hardware.xml mode change 100644 => 100755 crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp mode change 100644 => 100755 crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp mode change 100644 => 100755 crane_plus_control/include/crane_plus_control/visibility_control.h mode change 100644 => 100755 crane_plus_control/launch/crane_plus_control.launch.py mode change 100644 => 100755 crane_plus_control/package.xml mode change 100644 => 100755 crane_plus_control/src/crane_plus_driver.cpp mode change 100644 => 100755 crane_plus_control/src/crane_plus_hardware.cpp mode change 100644 => 100755 crane_plus_description/CHANGELOG.rst mode change 100644 => 100755 crane_plus_description/CMakeLists.txt mode change 100644 => 100755 crane_plus_description/README.md mode change 100644 => 100755 crane_plus_description/crane_plus_description/__init__.py mode change 100644 => 100755 crane_plus_description/launch/display.launch.py mode change 100644 => 100755 crane_plus_description/launch/display.rviz mode change 100644 => 100755 crane_plus_description/meshes/collision/Hand_collision_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/collision/Link1_collision_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/collision/Link2_collision_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/collision/Link3_collision_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/collision/Link4_collision_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/collision/body_collision_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/visual/Hand_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/visual/Link1_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/visual/Link2_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/visual/Link3_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/visual/Link4_20201013.stl mode change 100644 => 100755 crane_plus_description/meshes/visual/body_20201013.stl mode change 100644 => 100755 crane_plus_description/package.xml mode change 100644 => 100755 crane_plus_description/test/dummy_controllers.yaml mode change 100644 => 100755 crane_plus_description/test/test_robot_description_loader.py mode change 100644 => 100755 crane_plus_description/urdf/camera.urdf.xacro mode change 100644 => 100755 crane_plus_description/urdf/camera_stand.urdf.xacro mode change 100644 => 100755 crane_plus_description/urdf/crane_plus.gazebo.xacro mode change 100644 => 100755 crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro mode change 100644 => 100755 crane_plus_description/urdf/crane_plus.ros2_control.xacro mode change 100644 => 100755 crane_plus_description/urdf/crane_plus.urdf.xacro mode change 100644 => 100755 crane_plus_description/urdf/crane_plus.xacro mode change 100644 => 100755 crane_plus_examples/CHANGELOG.rst mode change 100644 => 100755 crane_plus_examples/CMakeLists.txt mode change 100644 => 100755 crane_plus_examples/README.md mode change 100644 => 100755 crane_plus_examples/aruco_markers.pdf mode change 100644 => 100755 crane_plus_examples/config/camera_info.yaml mode change 100644 => 100755 crane_plus_examples/launch/camera_example.launch.py mode change 100644 => 100755 crane_plus_examples/launch/camera_example.rviz mode change 100644 => 100755 crane_plus_examples/launch/demo.launch.py mode change 100644 => 100755 crane_plus_examples/launch/example.launch.py mode change 100644 => 100755 crane_plus_examples/package.xml mode change 100644 => 100755 crane_plus_examples/src/aruco_detection.cpp mode change 100644 => 100755 crane_plus_examples/src/color_detection.cpp mode change 100644 => 100755 crane_plus_examples/src/gripper_control.cpp mode change 100644 => 100755 crane_plus_examples/src/joint_values.cpp mode change 100644 => 100755 crane_plus_examples/src/pick_and_place.cpp mode change 100644 => 100755 crane_plus_examples/src/pick_and_place_tf.cpp mode change 100644 => 100755 crane_plus_examples/src/pose_groupstate.cpp mode change 100644 => 100755 crane_plus_gazebo/CHANGELOG.rst mode change 100644 => 100755 crane_plus_gazebo/CMakeLists.txt mode change 100644 => 100755 crane_plus_gazebo/README.md mode change 100644 => 100755 crane_plus_gazebo/gui/gui.config mode change 100644 => 100755 crane_plus_gazebo/launch/crane_plus_with_table.launch.py mode change 100644 => 100755 crane_plus_gazebo/package.xml mode change 100644 => 100755 crane_plus_gazebo/worlds/table.sdf delete mode 100755 crane_plus_moveit_config/.setup_assistant mode change 100644 => 100755 crane_plus_moveit_config/config/moveit.rviz mode change 100644 => 100755 crane_plus_moveit_config/config/pilz_cartesian_limits.yaml mode change 100644 => 100755 crane_plus_moveit_config/config/sensors_3d.yaml mode change 100644 => 100755 crane_plus_moveit_config/launch/demo.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/move_group.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/moveit_rviz.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/rsp.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/setup_assistant.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/spawn_controllers.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py mode change 100644 => 100755 crane_plus_moveit_config/launch/warehouse_db.launch.py delete mode 100644 install/COLCON_IGNORE delete mode 100644 install/_local_setup_util_ps1.py delete mode 100644 install/_local_setup_util_sh.py delete mode 100644 install/crane_plus_moveit_config/share/ament_index/resource_index/packages/crane_plus_moveit_config delete mode 100644 install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config delete mode 100644 install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml delete mode 100644 install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh delete mode 100644 install/local_setup.bash delete mode 100644 install/local_setup.ps1 delete mode 100644 install/local_setup.sh delete mode 100644 install/local_setup.zsh delete mode 100644 install/setup.bash delete mode 100644 install/setup.ps1 delete mode 100644 install/setup.sh delete mode 100644 install/setup.zsh delete mode 100644 log/COLCON_IGNORE delete mode 120000 log/latest delete mode 120000 log/latest_build diff --git a/.docker/README.md b/.docker/README.md old mode 100644 new mode 100755 diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile old mode 100644 new mode 100755 diff --git a/.dockerignore b/.dockerignore old mode 100644 new mode 100755 diff --git a/.github/workflows/build_docker_image.yml b/.github/workflows/build_docker_image.yml old mode 100644 new mode 100755 diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml old mode 100644 new mode 100755 diff --git a/.gitignore b/.gitignore old mode 100644 new mode 100755 index ba6ffecc..6d611533 --- a/.gitignore +++ b/.gitignore @@ -58,3 +58,9 @@ target/ .pytest_cache .vscode + +# unnecessary +crane_plus_moveit_config/.setup_assistant +install/ +log/ +crane_plus_examples/scripts/ diff --git a/LICENSE b/LICENSE old mode 100644 new mode 100755 diff --git a/README.md b/README.md old mode 100644 new mode 100755 diff --git a/crane_plus/CHANGELOG.rst b/crane_plus/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/crane_plus/CMakeLists.txt b/crane_plus/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/crane_plus/package.xml b/crane_plus/package.xml old mode 100644 new mode 100755 diff --git a/crane_plus_control/CHANGELOG.rst b/crane_plus_control/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/crane_plus_control/CMakeLists.txt b/crane_plus_control/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/crane_plus_control/README.md b/crane_plus_control/README.md old mode 100644 new mode 100755 diff --git a/crane_plus_control/config/crane_plus_controllers.yaml b/crane_plus_control/config/crane_plus_controllers.yaml old mode 100644 new mode 100755 diff --git a/crane_plus_control/crane_plus_hardware.xml b/crane_plus_control/crane_plus_hardware.xml old mode 100644 new mode 100755 diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp old mode 100644 new mode 100755 diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp old mode 100644 new mode 100755 diff --git a/crane_plus_control/include/crane_plus_control/visibility_control.h b/crane_plus_control/include/crane_plus_control/visibility_control.h old mode 100644 new mode 100755 diff --git a/crane_plus_control/launch/crane_plus_control.launch.py b/crane_plus_control/launch/crane_plus_control.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_control/package.xml b/crane_plus_control/package.xml old mode 100644 new mode 100755 diff --git a/crane_plus_control/src/crane_plus_driver.cpp b/crane_plus_control/src/crane_plus_driver.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_description/CHANGELOG.rst b/crane_plus_description/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/crane_plus_description/CMakeLists.txt b/crane_plus_description/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/crane_plus_description/README.md b/crane_plus_description/README.md old mode 100644 new mode 100755 diff --git a/crane_plus_description/crane_plus_description/__init__.py b/crane_plus_description/crane_plus_description/__init__.py old mode 100644 new mode 100755 diff --git a/crane_plus_description/launch/display.launch.py b/crane_plus_description/launch/display.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_description/launch/display.rviz b/crane_plus_description/launch/display.rviz old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/collision/Hand_collision_20201013.stl b/crane_plus_description/meshes/collision/Hand_collision_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/collision/Link1_collision_20201013.stl b/crane_plus_description/meshes/collision/Link1_collision_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/collision/Link2_collision_20201013.stl b/crane_plus_description/meshes/collision/Link2_collision_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/collision/Link3_collision_20201013.stl b/crane_plus_description/meshes/collision/Link3_collision_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/collision/Link4_collision_20201013.stl b/crane_plus_description/meshes/collision/Link4_collision_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/collision/body_collision_20201013.stl b/crane_plus_description/meshes/collision/body_collision_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/visual/Hand_20201013.stl b/crane_plus_description/meshes/visual/Hand_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/visual/Link1_20201013.stl b/crane_plus_description/meshes/visual/Link1_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/visual/Link2_20201013.stl b/crane_plus_description/meshes/visual/Link2_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/visual/Link3_20201013.stl b/crane_plus_description/meshes/visual/Link3_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/visual/Link4_20201013.stl b/crane_plus_description/meshes/visual/Link4_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/meshes/visual/body_20201013.stl b/crane_plus_description/meshes/visual/body_20201013.stl old mode 100644 new mode 100755 diff --git a/crane_plus_description/package.xml b/crane_plus_description/package.xml old mode 100644 new mode 100755 diff --git a/crane_plus_description/test/dummy_controllers.yaml b/crane_plus_description/test/dummy_controllers.yaml old mode 100644 new mode 100755 diff --git a/crane_plus_description/test/test_robot_description_loader.py b/crane_plus_description/test/test_robot_description_loader.py old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/camera.urdf.xacro b/crane_plus_description/urdf/camera.urdf.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/camera_stand.urdf.xacro b/crane_plus_description/urdf/camera_stand.urdf.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/crane_plus.gazebo.xacro b/crane_plus_description/urdf/crane_plus.gazebo.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro b/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/crane_plus.ros2_control.xacro b/crane_plus_description/urdf/crane_plus.ros2_control.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/crane_plus.urdf.xacro b/crane_plus_description/urdf/crane_plus.urdf.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_description/urdf/crane_plus.xacro b/crane_plus_description/urdf/crane_plus.xacro old mode 100644 new mode 100755 diff --git a/crane_plus_examples/CHANGELOG.rst b/crane_plus_examples/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/crane_plus_examples/CMakeLists.txt b/crane_plus_examples/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/crane_plus_examples/README.md b/crane_plus_examples/README.md old mode 100644 new mode 100755 diff --git a/crane_plus_examples/aruco_markers.pdf b/crane_plus_examples/aruco_markers.pdf old mode 100644 new mode 100755 diff --git a/crane_plus_examples/config/camera_info.yaml b/crane_plus_examples/config/camera_info.yaml old mode 100644 new mode 100755 diff --git a/crane_plus_examples/launch/camera_example.launch.py b/crane_plus_examples/launch/camera_example.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_examples/launch/camera_example.rviz b/crane_plus_examples/launch/camera_example.rviz old mode 100644 new mode 100755 diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_examples/launch/example.launch.py b/crane_plus_examples/launch/example.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_examples/package.xml b/crane_plus_examples/package.xml old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_examples/src/pose_groupstate.cpp b/crane_plus_examples/src/pose_groupstate.cpp old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/CHANGELOG.rst b/crane_plus_gazebo/CHANGELOG.rst old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/CMakeLists.txt b/crane_plus_gazebo/CMakeLists.txt old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/README.md b/crane_plus_gazebo/README.md old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/gui/gui.config b/crane_plus_gazebo/gui/gui.config old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/launch/crane_plus_with_table.launch.py b/crane_plus_gazebo/launch/crane_plus_with_table.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/package.xml b/crane_plus_gazebo/package.xml old mode 100644 new mode 100755 diff --git a/crane_plus_gazebo/worlds/table.sdf b/crane_plus_gazebo/worlds/table.sdf old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/.setup_assistant b/crane_plus_moveit_config/.setup_assistant deleted file mode 100755 index 3058c8a2..00000000 --- a/crane_plus_moveit_config/.setup_assistant +++ /dev/null @@ -1,25 +0,0 @@ -moveit_setup_assistant_config: - urdf: - package: crane_plus_description - relative_path: urdf/crane_plus.urdf.xacro - srdf: - relative_path: config/crane_plus.srdf - package_settings: - author_name: RT Corporation - author_email: shop@rt-net.jp - generated_timestamp: 1721799427 - control_xacro: - command: - - position - state: - - position - - velocity - modified_urdf: - xacros: - - control_xacro - control_xacro: - command: - - position - state: - - position - - velocity \ No newline at end of file diff --git a/crane_plus_moveit_config/config/moveit.rviz b/crane_plus_moveit_config/config/moveit.rviz old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml b/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/config/sensors_3d.yaml b/crane_plus_moveit_config/config/sensors_3d.yaml old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/demo.launch.py b/crane_plus_moveit_config/launch/demo.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/move_group.launch.py b/crane_plus_moveit_config/launch/move_group.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/moveit_rviz.launch.py b/crane_plus_moveit_config/launch/moveit_rviz.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/rsp.launch.py b/crane_plus_moveit_config/launch/rsp.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/spawn_controllers.launch.py b/crane_plus_moveit_config/launch/spawn_controllers.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py b/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py old mode 100644 new mode 100755 diff --git a/crane_plus_moveit_config/launch/warehouse_db.launch.py b/crane_plus_moveit_config/launch/warehouse_db.launch.py old mode 100644 new mode 100755 diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py deleted file mode 100644 index 3c6d9e87..00000000 --- a/install/_local_setup_util_ps1.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' -FORMAT_STR_USE_ENV_VAR = '$env:{name}' -FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py deleted file mode 100644 index f67eaa98..00000000 --- a/install/_local_setup_util_sh.py +++ /dev/null @@ -1,407 +0,0 @@ -# Copyright 2016-2019 Dirk Thomas -# Licensed under the Apache License, Version 2.0 - -import argparse -from collections import OrderedDict -import os -from pathlib import Path -import sys - - -FORMAT_STR_COMMENT_LINE = '# {comment}' -FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' -FORMAT_STR_USE_ENV_VAR = '${name}' -FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 -FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 -FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 - -DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' -DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' -DSV_TYPE_SET = 'set' -DSV_TYPE_SET_IF_UNSET = 'set-if-unset' -DSV_TYPE_SOURCE = 'source' - - -def main(argv=sys.argv[1:]): # noqa: D103 - parser = argparse.ArgumentParser( - description='Output shell commands for the packages in topological ' - 'order') - parser.add_argument( - 'primary_extension', - help='The file extension of the primary shell') - parser.add_argument( - 'additional_extension', nargs='?', - help='The additional file extension to be considered') - parser.add_argument( - '--merged-install', action='store_true', - help='All install prefixes are merged into a single location') - args = parser.parse_args(argv) - - packages = get_packages(Path(__file__).parent, args.merged_install) - - ordered_packages = order_packages(packages) - for pkg_name in ordered_packages: - if _include_comments(): - print( - FORMAT_STR_COMMENT_LINE.format_map( - {'comment': 'Package: ' + pkg_name})) - prefix = os.path.abspath(os.path.dirname(__file__)) - if not args.merged_install: - prefix = os.path.join(prefix, pkg_name) - for line in get_commands( - pkg_name, prefix, args.primary_extension, - args.additional_extension - ): - print(line) - - for line in _remove_ending_separators(): - print(line) - - -def get_packages(prefix_path, merged_install): - """ - Find packages based on colcon-specific files created during installation. - - :param Path prefix_path: The install prefix path of all packages - :param bool merged_install: The flag if the packages are all installed - directly in the prefix or if each package is installed in a subdirectory - named after the package - :returns: A mapping from the package name to the set of runtime - dependencies - :rtype: dict - """ - packages = {} - # since importing colcon_core isn't feasible here the following constant - # must match colcon_core.location.get_relative_package_index_path() - subdirectory = 'share/colcon-core/packages' - if merged_install: - # return if workspace is empty - if not (prefix_path / subdirectory).is_dir(): - return packages - # find all files in the subdirectory - for p in (prefix_path / subdirectory).iterdir(): - if not p.is_file(): - continue - if p.name.startswith('.'): - continue - add_package_runtime_dependencies(p, packages) - else: - # for each subdirectory look for the package specific file - for p in prefix_path.iterdir(): - if not p.is_dir(): - continue - if p.name.startswith('.'): - continue - p = p / subdirectory / p.name - if p.is_file(): - add_package_runtime_dependencies(p, packages) - - # remove unknown dependencies - pkg_names = set(packages.keys()) - for k in packages.keys(): - packages[k] = {d for d in packages[k] if d in pkg_names} - - return packages - - -def add_package_runtime_dependencies(path, packages): - """ - Check the path and if it exists extract the packages runtime dependencies. - - :param Path path: The resource file containing the runtime dependencies - :param dict packages: A mapping from package names to the sets of runtime - dependencies to add to - """ - content = path.read_text() - dependencies = set(content.split(os.pathsep) if content else []) - packages[path.name] = dependencies - - -def order_packages(packages): - """ - Order packages topologically. - - :param dict packages: A mapping from package name to the set of runtime - dependencies - :returns: The package names - :rtype: list - """ - # select packages with no dependencies in alphabetical order - to_be_ordered = list(packages.keys()) - ordered = [] - while to_be_ordered: - pkg_names_without_deps = [ - name for name in to_be_ordered if not packages[name]] - if not pkg_names_without_deps: - reduce_cycle_set(packages) - raise RuntimeError( - 'Circular dependency between: ' + ', '.join(sorted(packages))) - pkg_names_without_deps.sort() - pkg_name = pkg_names_without_deps[0] - to_be_ordered.remove(pkg_name) - ordered.append(pkg_name) - # remove item from dependency lists - for k in list(packages.keys()): - if pkg_name in packages[k]: - packages[k].remove(pkg_name) - return ordered - - -def reduce_cycle_set(packages): - """ - Reduce the set of packages to the ones part of the circular dependency. - - :param dict packages: A mapping from package name to the set of runtime - dependencies which is modified in place - """ - last_depended = None - while len(packages) > 0: - # get all remaining dependencies - depended = set() - for pkg_name, dependencies in packages.items(): - depended = depended.union(dependencies) - # remove all packages which are not dependent on - for name in list(packages.keys()): - if name not in depended: - del packages[name] - if last_depended: - # if remaining packages haven't changed return them - if last_depended == depended: - return packages.keys() - # otherwise reduce again - last_depended = depended - - -def _include_comments(): - # skipping comment lines when COLCON_TRACE is not set speeds up the - # processing especially on Windows - return bool(os.environ.get('COLCON_TRACE')) - - -def get_commands(pkg_name, prefix, primary_extension, additional_extension): - commands = [] - package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') - if os.path.exists(package_dsv_path): - commands += process_dsv_file( - package_dsv_path, prefix, primary_extension, additional_extension) - return commands - - -def process_dsv_file( - dsv_path, prefix, primary_extension=None, additional_extension=None -): - commands = [] - if _include_comments(): - commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) - with open(dsv_path, 'r') as h: - content = h.read() - lines = content.splitlines() - - basenames = OrderedDict() - for i, line in enumerate(lines): - # skip over empty or whitespace-only lines - if not line.strip(): - continue - # skip over comments - if line.startswith('#'): - continue - try: - type_, remainder = line.split(';', 1) - except ValueError: - raise RuntimeError( - "Line %d in '%s' doesn't contain a semicolon separating the " - 'type from the arguments' % (i + 1, dsv_path)) - if type_ != DSV_TYPE_SOURCE: - # handle non-source lines - try: - commands += handle_dsv_types_except_source( - type_, remainder, prefix) - except RuntimeError as e: - raise RuntimeError( - "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e - else: - # group remaining source lines by basename - path_without_ext, ext = os.path.splitext(remainder) - if path_without_ext not in basenames: - basenames[path_without_ext] = set() - assert ext.startswith('.') - ext = ext[1:] - if ext in (primary_extension, additional_extension): - basenames[path_without_ext].add(ext) - - # add the dsv extension to each basename if the file exists - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if os.path.exists(basename + '.dsv'): - extensions.add('dsv') - - for basename, extensions in basenames.items(): - if not os.path.isabs(basename): - basename = os.path.join(prefix, basename) - if 'dsv' in extensions: - # process dsv files recursively - commands += process_dsv_file( - basename + '.dsv', prefix, primary_extension=primary_extension, - additional_extension=additional_extension) - elif primary_extension in extensions and len(extensions) == 1: - # source primary-only files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + primary_extension})] - elif additional_extension in extensions: - # source non-primary files - commands += [ - FORMAT_STR_INVOKE_SCRIPT.format_map({ - 'prefix': prefix, - 'script_path': basename + '.' + additional_extension})] - - return commands - - -def handle_dsv_types_except_source(type_, remainder, prefix): - commands = [] - if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): - try: - env_name, value = remainder.split(';', 1) - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the value') - try_prefixed_value = os.path.join(prefix, value) if value else prefix - if os.path.exists(try_prefixed_value): - value = try_prefixed_value - if type_ == DSV_TYPE_SET: - commands += _set(env_name, value) - elif type_ == DSV_TYPE_SET_IF_UNSET: - commands += _set_if_unset(env_name, value) - else: - assert False - elif type_ in ( - DSV_TYPE_APPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE, - DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS - ): - try: - env_name_and_values = remainder.split(';') - except ValueError: - raise RuntimeError( - "doesn't contain a semicolon separating the environment name " - 'from the values') - env_name = env_name_and_values[0] - values = env_name_and_values[1:] - for value in values: - if not value: - value = prefix - elif not os.path.isabs(value): - value = os.path.join(prefix, value) - if ( - type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and - not os.path.exists(value) - ): - comment = f'skip extending {env_name} with not existing ' \ - f'path: {value}' - if _include_comments(): - commands.append( - FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) - elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: - commands += _append_unique_value(env_name, value) - else: - commands += _prepend_unique_value(env_name, value) - else: - raise RuntimeError( - 'contains an unknown environment hook type: ' + type_) - return commands - - -env_state = {} - - -def _append_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # append even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional leading separator - extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': extend + value}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -def _prepend_unique_value(name, value): - global env_state - if name not in env_state: - if os.environ.get(name): - env_state[name] = set(os.environ[name].split(os.pathsep)) - else: - env_state[name] = set() - # prepend even if the variable has not been set yet, in case a shell script sets the - # same variable without the knowledge of this Python script. - # later _remove_ending_separators() will cleanup any unintentional trailing separator - extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value + extend}) - if value not in env_state[name]: - env_state[name].add(value) - else: - if not _include_comments(): - return [] - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -# generate commands for removing prepended underscores -def _remove_ending_separators(): - # do nothing if the shell extension does not implement the logic - if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: - return [] - - global env_state - commands = [] - for name in env_state: - # skip variables that already had values before this script started prepending - if name in os.environ: - continue - commands += [ - FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), - FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] - return commands - - -def _set(name, value): - global env_state - env_state[name] = value - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - return [line] - - -def _set_if_unset(name, value): - global env_state - line = FORMAT_STR_SET_ENV_VAR.format_map( - {'name': name, 'value': value}) - if env_state.get(name, os.environ.get(name)): - line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) - return [line] - - -if __name__ == '__main__': # pragma: no cover - try: - rc = main() - except RuntimeError as e: - print(str(e), file=sys.stderr) - rc = 1 - sys.exit(rc) diff --git a/install/crane_plus_moveit_config/share/ament_index/resource_index/packages/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/ament_index/resource_index/packages/crane_plus_moveit_config deleted file mode 100644 index e69de29b..00000000 diff --git a/install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config deleted file mode 100644 index 55163222..00000000 --- a/install/crane_plus_moveit_config/share/ament_index/resource_index/parent_prefix_path/crane_plus_moveit_config +++ /dev/null @@ -1 +0,0 @@ -/home/user/overlay_ws/install/crane_plus_moveit_config_new:/home/user/overlay_ws/install/crane_plus:/home/user/overlay_ws/install/crane_plus_gazebo:/home/user/overlay_ws/install/crane_plus_examples:/home/user/overlay_ws/install/crane_plus_moveit_config:/home/user/overlay_ws/install/crane_plus_control:/home/user/overlay_ws/install/crane_plus_description:/opt/ros/jazzy \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config b/install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config deleted file mode 100644 index c3a13fbd..00000000 --- a/install/crane_plus_moveit_config/share/colcon-core/packages/crane_plus_moveit_config +++ /dev/null @@ -1 +0,0 @@ -controller_manager:crane_plus_description:joint_state_publisher:joint_state_publisher_gui:moveit_configs_utils:moveit_kinematics:moveit_planners:moveit_ros_move_group:moveit_ros_visualization:moveit_ros_warehouse:moveit_setup_assistant:moveit_simple_controller_manager:robot_state_publisher:rviz2:rviz_common:rviz_default_plugins:tf2_ros:warehouse_ros_mongo:xacro \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant deleted file mode 100644 index b7b0cc0e..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/.setup_assistant +++ /dev/null @@ -1,10 +0,0 @@ -moveit_setup_assistant_config: - urdf: - package: crane_plus_description - relative_path: urdf/crane_plus.urdf.xacro - srdf: - relative_path: config/crane_plus.srdf - package_settings: - author_name: RT Corporation - author_email: shop@rt-net.jp - generated_timestamp: 1721806216 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake deleted file mode 100644 index 96789497..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig-version.cmake +++ /dev/null @@ -1,14 +0,0 @@ -# generated from ament/cmake/core/templates/nameConfig-version.cmake.in -set(PACKAGE_VERSION "0.3.0") - -set(PACKAGE_VERSION_EXACT False) -set(PACKAGE_VERSION_COMPATIBLE False) - -if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}") - set(PACKAGE_VERSION_EXACT True) - set(PACKAGE_VERSION_COMPATIBLE True) -endif() - -if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}") - set(PACKAGE_VERSION_COMPATIBLE True) -endif() diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake deleted file mode 100644 index d79b9053..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/cmake/crane_plus_moveit_configConfig.cmake +++ /dev/null @@ -1,42 +0,0 @@ -# generated from ament/cmake/core/templates/nameConfig.cmake.in - -# prevent multiple inclusion -if(_crane_plus_moveit_config_CONFIG_INCLUDED) - # ensure to keep the found flag the same - if(NOT DEFINED crane_plus_moveit_config_FOUND) - # explicitly set it to FALSE, otherwise CMake will set it to TRUE - set(crane_plus_moveit_config_FOUND FALSE) - elseif(NOT crane_plus_moveit_config_FOUND) - # use separate condition to avoid uninitialized variable warning - set(crane_plus_moveit_config_FOUND FALSE) - endif() - return() -endif() -set(_crane_plus_moveit_config_CONFIG_INCLUDED TRUE) - -# output package information -if(NOT crane_plus_moveit_config_FIND_QUIETLY) - message(STATUS "Found crane_plus_moveit_config: 0.3.0 (${crane_plus_moveit_config_DIR})") -endif() - -# warn when using a deprecated package -if(NOT "" STREQUAL "") - set(_msg "Package 'crane_plus_moveit_config' is deprecated") - # append custom deprecation text if available - if(NOT "" STREQUAL "TRUE") - set(_msg "${_msg} ()") - endif() - # optionally quiet the deprecation message - if(NOT crane_plus_moveit_config_DEPRECATED_QUIET) - message(DEPRECATION "${_msg}") - endif() -endif() - -# flag package as ament-based to distinguish it after being find_package()-ed -set(crane_plus_moveit_config_FOUND_AMENT_PACKAGE TRUE) - -# include all config extra files -set(_extras "") -foreach(_extra ${_extras}) - include("${crane_plus_moveit_config_DIR}/${_extra}") -endforeach() diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml deleted file mode 100644 index e108e7b3..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,27 +0,0 @@ -trajectory_execution: - - allowed_execution_duration_scaling: 1.2 - - allowed_goal_duration_margin: 0.5 - - allowed_start_tolerance: 0.1 - -moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager - -controller_names: - - crane_plus_arm_controller - - crane_plus_gripper_controller - -crane_plus_arm_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - crane_plus_joint1 - - crane_plus_joint2 - - crane_plus_joint3 - - crane_plus_joint4 - -crane_plus_gripper_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - crane_plus_joint_hand diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf deleted file mode 100644 index af40281f..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/crane_plus.srdf +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml deleted file mode 100644 index bd4b95ae..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/joint_limits.yaml +++ /dev/null @@ -1,35 +0,0 @@ -# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed - -# For beginners, we downscale velocity and acceleration limits. -# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 - -# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] -# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] -joint_limits: - crane_plus_joint1: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 - crane_plus_joint2: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 - crane_plus_joint3: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 - crane_plus_joint4: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 - crane_plus_joint_hand: - has_velocity_limits: true - max_velocity: 2 - has_acceleration_limits: false - max_acceleration: 0 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml deleted file mode 100644 index 193fbb96..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/kinematics.yaml +++ /dev/null @@ -1,4 +0,0 @@ -arm: - kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin - kinematics_solver_search_resolution: 0.0050000000000000001 - kinematics_solver_timeout: 0.0050000000000000001 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz deleted file mode 100644 index f31651ed..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/moveit.rviz +++ /dev/null @@ -1,51 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Name: Displays - Property Tree Widget: - Expanded: - - /MotionPlanning1 - - Class: rviz_common/Help - Name: Help - - Class: rviz_common/Views - Name: Views -Visualization Manager: - Displays: - - Class: rviz_default_plugins/Grid - Name: Grid - Value: true - - Class: moveit_rviz_plugin/MotionPlanning - Name: MotionPlanning - Planned Path: - Loop Animation: true - State Display Time: 0.05 s - Trajectory Topic: display_planned_path - Planning Scene Topic: monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 1 - Scene Robot: - Robot Alpha: 0.5 - Value: true - Global Options: - Fixed Frame: world - Tools: - - Class: rviz_default_plugins/Interact - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 2.0 - Focal Point: - X: -0.1 - Y: 0.25 - Z: 0.30 - Name: Current View - Pitch: 0.5 - Target Frame: world - Yaw: -0.623 -Window Geometry: - Height: 975 - QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Width: 1200 diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml deleted file mode 100644 index f32aafdf..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/ompl_planning.yaml +++ /dev/null @@ -1,196 +0,0 @@ -planning_plugins: - - ompl_interface/OMPLPlanner -# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline. -request_adapters: - # - default_planning_request_adapters/ResolveConstraintFrames - # - default_planning_request_adapters/ValidateWorkspaceBounds - # - default_planning_request_adapters/CheckStartStateBounds - # - default_planning_request_adapters/CheckStartStateCollision - - default_planning_request_adapters/AddTimeOptimalParameterization - - default_planning_request_adapters/FixWorkspaceBounds - - default_planning_request_adapters/FixStartStateBounds - - default_planning_request_adapters/FixStartStatePathConstraints -response_adapters: - # - default_planning_response_adapters/AddTimeOptimalParameterization - # - default_planning_response_adapters/ValidateSolution - # - default_planning_response_adapters/DisplayMotionPath - -start_state_max_bounds_error: 0.1 - -planner_configs: - 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 - -arm: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault -gripper: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml deleted file mode 100644 index b2997caf..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Limits for the Pilz planner -cartesian_limits: - max_trans_vel: 1.0 - max_trans_acc: 2.25 - max_trans_dec: -5.0 - max_rot_vel: 1.57 diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml deleted file mode 100644 index 5d90e51d..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/config/sensors_3d.yaml +++ /dev/null @@ -1,23 +0,0 @@ -sensors: - - kinect_pointcloud - - kinect_depthimage -kinect_pointcloud: - filtered_cloud_topic: filtered_cloud - max_range: 5.0 - max_update_rate: 1.0 - padding_offset: 0.1 - padding_scale: 1.0 - point_cloud_topic: /head_mount_kinect/depth_registered/points - point_subsample: 1 - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater -kinect_depthimage: - far_clipping_plane_distance: 5.0 - filtered_cloud_topic: filtered_cloud - image_topic: /head_mount_kinect/depth_registered/image_raw - max_update_rate: 1.0 - near_clipping_plane_distance: 0.3 - padding_offset: 0.03 - padding_scale: 4.0 - queue_size: 5 - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - shadow_threshold: 0.2 \ No newline at end of file diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv deleted file mode 100644 index 79d4c95b..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.dsv +++ /dev/null @@ -1 +0,0 @@ -prepend-non-duplicate;AMENT_PREFIX_PATH; diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh deleted file mode 100644 index 02e441b7..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/ament_prefix_path.sh +++ /dev/null @@ -1,4 +0,0 @@ -# copied from -# ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh - -ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX" diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv deleted file mode 100644 index b94426af..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.dsv +++ /dev/null @@ -1 +0,0 @@ -prepend-non-duplicate-if-exists;PATH;bin diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh deleted file mode 100644 index e59b749a..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/environment/path.sh +++ /dev/null @@ -1,5 +0,0 @@ -# copied from ament_cmake_core/cmake/environment_hooks/environment/path.sh - -if [ -d "$AMENT_CURRENT_PREFIX/bin" ]; then - ament_prepend_unique_value PATH "$AMENT_CURRENT_PREFIX/bin" -fi diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv deleted file mode 100644 index e119f32c..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv +++ /dev/null @@ -1 +0,0 @@ -prepend-non-duplicate;CMAKE_PREFIX_PATH; diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 deleted file mode 100644 index d03facc1..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 +++ /dev/null @@ -1,3 +0,0 @@ -# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em - -colcon_prepend_unique_value CMAKE_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX" diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh deleted file mode 100644 index a948e685..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh +++ /dev/null @@ -1,3 +0,0 @@ -# generated from colcon_core/shell/template/hook_prepend_value.sh.em - -_colcon_prepend_unique_value CMAKE_PREFIX_PATH "$COLCON_CURRENT_PREFIX" diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py deleted file mode 100644 index 2b134ee0..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/demo.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_demo_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_demo_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py deleted file mode 100644 index d05ee6c5..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/move_group.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_move_group_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_move_group_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py deleted file mode 100644 index 9034635d..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/moveit_rviz.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_moveit_rviz_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_moveit_rviz_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py deleted file mode 100644 index 28439b32..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/rsp.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_rsp_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_rsp_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py deleted file mode 100644 index 398f8808..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.launch.py +++ /dev/null @@ -1,237 +0,0 @@ -# Copyright 2020 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 -import yaml -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 - -# Reference: https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes/ -# run_move_group/launch/run_move_group.launch.py - - -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(): - declare_loaded_description = DeclareLaunchArgument( - "loaded_description", - default_value="", - description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.", - ) - - declare_rviz_config_file = DeclareLaunchArgument( - "rviz_config_file", - default_value=get_package_share_directory("crane_plus_moveit_config") - + "/launch/run_move_group.rviz", - description="Set the path to rviz configuration file.", - ) - - # robot_description = {"robot_description": LaunchConfiguration("loaded_description")} - - # robot_description_semantic_config = load_file( - # "crane_plus_moveit_config", "config/crane_plus.srdf" - # ) - # robot_description_semantic = { - # "robot_description_semantic": robot_description_semantic_config - # } - - # kinematics_yaml = load_yaml("crane_plus_moveit_config", "config/kinematics.yaml") - - # Planning Functionality - # ompl_planning_pipeline_config = {'move_group': { - # 'planning_plugin': 'ompl_interface/OMPLPlanner', - # 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization \ - # default_planner_request_adapters/FixWorkspaceBounds \ - # default_planner_request_adapters/FixStartStateBounds \ - # default_planner_request_adapters/FixStartStateCollision \ - # default_planner_request_adapters/FixStartStatePathConstraints', - # 'start_state_max_bounds_error': 0.1}} - # ompl_planning_yaml = load_yaml('crane_plus_moveit_config', 'config/ompl_planning.yaml') - # ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - # controllers_yaml = load_yaml('crane_plus_moveit_config', 'config/controllers.yaml') - # moveit_controllers = { - # 'moveit_simple_controller_manager': controllers_yaml, - # 'moveit_controller_manager': - # 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} - - # trajectory_execution= {'moveit_manage_controllers': True, - # 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - # 'trajectory_execution.allowed_goal_duration_margin': 0.5, - # 'trajectory_execution.allowed_start_tolerance': 0.1} - - # planning_scene_monitor_parameters = {'publish_planning_scene': True, - # 'publish_geometry_updates': True, - # 'publish_state_updates': True, - # 'publish_transforms_updates': True} - - - # MoveItConfigBuilderによるパラメータ設定 - moveit_config = ( - # ロボット名の定義 - MoveItConfigsBuilder("crane_plus") - # URDFの設定 - .robot_description( - mappings={"robot_description": LaunchConfiguration("loaded_description")} - ) - # SRDFの設定 - .robot_description_semantic(file_path="config/crane_plus.srdf") - # キネマティクスの設定 - .robot_description_kinematics(file_path="config/kinematics.yaml") - # 軌道計画のプラグイン設定 - .planning_pipelines( - pipelines=["ompl"], - default_planning_pipeline="ompl", - ) - # 軌道追従制御ノードの設定 - .trajectory_execution( - # file_path="controllers.yaml", - file_path="config/controllers.yaml", - moveit_manage_controllers=True, - ) - .joint_limits( - file_path="config/joint_limits.yaml" - ) - # Planning Sceneのトピックの設定 - .planning_scene_monitor( - publish_planning_scene=True, - publish_geometry_updates=True, - publish_state_updates=True, - publish_transforms_updates=True, - ).to_moveit_configs() - ) - - # Start the actual move_group node/action server - # run_move_group_node = Node(package='moveit_ros_move_group', - # executable='move_group', - # output='screen', - # parameters=[robot_description, - # robot_description_semantic, - # kinematics_yaml, - # ompl_planning_pipeline_config, - # trajectory_execution, - # moveit_controllers, - # planning_scene_monitor_parameters]) - - # Start the actual move_group node/action server - # パラメータをMoveItConfigsBuilderを使った渡し方に変更 - run_move_group_node = Node( - package="moveit_ros_move_group", - executable="move_group", - output="screen", - parameters=[moveit_config.to_dict()], - arguments=["--ros-args", "--log-level", "info"], - ) - - # RViz - rviz_config_file = LaunchConfiguration("rviz_config_file") - # rviz_node = Node(package='rviz2', - # executable='rviz2', - # name='rviz2', - # output='log', - # arguments=['-d', rviz_config_file], - # parameters=[robot_description, - # robot_description_semantic, - # ompl_planning_pipeline_config, - # kinematics_yaml]) - - # MoveitConfigsBuilderのparameter指定に書き直す - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.planning_pipelines, - moveit_config.robot_description_kinematics, - moveit_config.joint_limits, - ], - ) - - # Static TF - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], - ) - - # Publish TF - # MoveitConfigsBuilderのparameter指定に書き直す - # robot_state_publisher = Node( - # package="robot_state_publisher", - # executable="robot_state_publisher", - # name="robot_state_publisher", - # output="both", - # parameters=[robot_description], - # ) - - # Publish TF - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="both", - parameters=[moveit_config.robot_description], - ) - - return LaunchDescription( - [ - declare_loaded_description, - declare_rviz_config_file, - run_move_group_node, - rviz_node, - static_tf, - robot_state_publisher, - ] - ) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz deleted file mode 100644 index 44b5c6a9..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/run_move_group.rviz +++ /dev/null @@ -1,351 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /MotionPlanning1 - - /Trajectory1 - Splitter Ratio: 0.5 - Tree Height: 295 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Acceleration_Scaling_Factor: 1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - crane_plus_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: /move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: arm - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - crane_plus_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 1 - - Class: moveit_rviz_plugin/Trajectory - Color Enabled: false - Enabled: true - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - crane_plus_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Name: Trajectory - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Robot Description: robot_description - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: display_planned_path - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 0.8449440598487854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.04136588051915169 - Y: -0.017012163996696472 - Z: 0.08837244659662247 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.44539815187454224 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.5553978681564331 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 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 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Trajectory - Trajectory Slider: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py deleted file mode 100644 index e16485a5..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/setup_assistant.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_setup_assistant_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_setup_assistant_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py deleted file mode 100644 index 85a8dfc7..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/spawn_controllers.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_spawn_controllers_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_spawn_controllers_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py deleted file mode 100644 index 7ff90d29..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py deleted file mode 100644 index 92981372..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/launch/warehouse_db.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_warehouse_db_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_warehouse_db_launch(moveit_config) diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash deleted file mode 100644 index 49782f24..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.bash +++ /dev/null @@ -1,46 +0,0 @@ -# generated from ament_package/template/package_level/local_setup.bash.in - -# source local_setup.sh from same directory as this file -_this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd) -# provide AMENT_CURRENT_PREFIX to shell script -AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd) -# store AMENT_CURRENT_PREFIX to restore it before each environment hook -_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX - -# trace output -if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then - echo "# . \"$_this_path/local_setup.sh\"" -fi -. "$_this_path/local_setup.sh" -unset _this_path - -# unset AMENT_ENVIRONMENT_HOOKS -# if not appending to them for return -if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then - unset AMENT_ENVIRONMENT_HOOKS -fi - -# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks -AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX -# list all environment hooks of this package - -# source all shell-specific environment hooks of this package -# if not returning them -if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then - _package_local_setup_IFS=$IFS - IFS=":" - for _hook in $AMENT_ENVIRONMENT_HOOKS; do - # restore AMENT_CURRENT_PREFIX for each environment hook - AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX - # restore IFS before sourcing other files - IFS=$_package_local_setup_IFS - . "$_hook" - done - unset _hook - IFS=$_package_local_setup_IFS - unset _package_local_setup_IFS - unset AMENT_ENVIRONMENT_HOOKS -fi - -unset _package_local_setup_AMENT_CURRENT_PREFIX -unset AMENT_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv deleted file mode 100644 index a5047579..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.dsv +++ /dev/null @@ -1,2 +0,0 @@ -source;share/crane_plus_moveit_config/environment/ament_prefix_path.sh -source;share/crane_plus_moveit_config/environment/path.sh diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh deleted file mode 100644 index 4f258b12..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.sh +++ /dev/null @@ -1,184 +0,0 @@ -# generated from ament_package/template/package_level/local_setup.sh.in - -# since this file is sourced use either the provided AMENT_CURRENT_PREFIX -# or fall back to the destination set at configure time -: ${AMENT_CURRENT_PREFIX:="/home/user/overlay_ws/src/crane_plus/install/crane_plus_moveit_config"} -if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then - if [ -z "$COLCON_CURRENT_PREFIX" ]; then - echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \ - "exist. Consider sourcing a different extension than '.sh'." 1>&2 - else - AMENT_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" - fi -fi - -# function to append values to environment variables -# using colons as separators and avoiding leading separators -ament_append_value() { - # arguments - _listname="$1" - _value="$2" - #echo "listname $_listname" - #eval echo "list value \$$_listname" - #echo "value $_value" - - # avoid leading separator - eval _values=\"\$$_listname\" - if [ -z "$_values" ]; then - eval export $_listname=\"$_value\" - #eval echo "set list \$$_listname" - else - # field separator must not be a colon - _ament_append_value_IFS=$IFS - unset IFS - eval export $_listname=\"\$$_listname:$_value\" - #eval echo "append list \$$_listname" - IFS=$_ament_append_value_IFS - unset _ament_append_value_IFS - fi - unset _values - - unset _value - unset _listname -} - -# function to append non-duplicate values to environment variables -# using colons as separators and avoiding leading separators -ament_append_unique_value() { - # arguments - _listname=$1 - _value=$2 - #echo "listname $_listname" - #eval echo "list value \$$_listname" - #echo "value $_value" - - # check if the list contains the value - eval _values=\$$_listname - _duplicate= - _ament_append_unique_value_IFS=$IFS - IFS=":" - if [ "$AMENT_SHELL" = "zsh" ]; then - ament_zsh_to_array _values - fi - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - if [ $_item = $_value ]; then - _duplicate=1 - fi - done - unset _item - - # append only non-duplicates - if [ -z "$_duplicate" ]; then - # avoid leading separator - if [ -z "$_values" ]; then - eval $_listname=\"$_value\" - #eval echo "set list \$$_listname" - else - # field separator must not be a colon - unset IFS - eval $_listname=\"\$$_listname:$_value\" - #eval echo "append list \$$_listname" - fi - fi - IFS=$_ament_append_unique_value_IFS - unset _ament_append_unique_value_IFS - unset _duplicate - unset _values - - unset _value - unset _listname -} - -# function to prepend non-duplicate values to environment variables -# using colons as separators and avoiding trailing separators -ament_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - #echo "listname $_listname" - #eval echo "list value \$$_listname" - #echo "value $_value" - - # check if the list contains the value - eval _values=\"\$$_listname\" - _duplicate= - _ament_prepend_unique_value_IFS=$IFS - IFS=":" - if [ "$AMENT_SHELL" = "zsh" ]; then - ament_zsh_to_array _values - fi - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - if [ "$_item" = "$_value" ]; then - _duplicate=1 - fi - done - unset _item - - # prepend only non-duplicates - if [ -z "$_duplicate" ]; then - # avoid trailing separator - if [ -z "$_values" ]; then - eval export $_listname=\"$_value\" - #eval echo "set list \$$_listname" - else - # field separator must not be a colon - unset IFS - eval export $_listname=\"$_value:\$$_listname\" - #eval echo "prepend list \$$_listname" - fi - fi - IFS=$_ament_prepend_unique_value_IFS - unset _ament_prepend_unique_value_IFS - unset _duplicate - unset _values - - unset _value - unset _listname -} - -# unset AMENT_ENVIRONMENT_HOOKS -# if not appending to them for return -if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then - unset AMENT_ENVIRONMENT_HOOKS -fi - -# list all environment hooks of this package -ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/crane_plus_moveit_config/environment/ament_prefix_path.sh" -ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/crane_plus_moveit_config/environment/path.sh" - -# source all shell-specific environment hooks of this package -# if not returning them -if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then - _package_local_setup_IFS=$IFS - IFS=":" - if [ "$AMENT_SHELL" = "zsh" ]; then - ament_zsh_to_array AMENT_ENVIRONMENT_HOOKS - fi - for _hook in $AMENT_ENVIRONMENT_HOOKS; do - if [ -f "$_hook" ]; then - # restore IFS before sourcing other files - IFS=$_package_local_setup_IFS - # trace output - if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then - echo "# . \"$_hook\"" - fi - . "$_hook" - fi - done - unset _hook - IFS=$_package_local_setup_IFS - unset _package_local_setup_IFS - unset AMENT_ENVIRONMENT_HOOKS -fi - -# reset AMENT_CURRENT_PREFIX after each package -# allowing to source multiple package-level setup files -unset AMENT_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh deleted file mode 100644 index fe161be5..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/local_setup.zsh +++ /dev/null @@ -1,59 +0,0 @@ -# generated from ament_package/template/package_level/local_setup.zsh.in - -AMENT_SHELL=zsh - -# source local_setup.sh from same directory as this file -_this_path=$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd) -# provide AMENT_CURRENT_PREFIX to shell script -AMENT_CURRENT_PREFIX=$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd) -# store AMENT_CURRENT_PREFIX to restore it before each environment hook -_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX - -# function to convert array-like strings into arrays -# to wordaround SH_WORD_SPLIT not being set -ament_zsh_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# trace output -if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then - echo "# . \"$_this_path/local_setup.sh\"" -fi -# the package-level local_setup file unsets AMENT_CURRENT_PREFIX -. "$_this_path/local_setup.sh" -unset _this_path - -# unset AMENT_ENVIRONMENT_HOOKS -# if not appending to them for return -if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then - unset AMENT_ENVIRONMENT_HOOKS -fi - -# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks -AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX -# list all environment hooks of this package - -# source all shell-specific environment hooks of this package -# if not returning them -if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then - _package_local_setup_IFS=$IFS - IFS=":" - for _hook in $AMENT_ENVIRONMENT_HOOKS; do - # restore AMENT_CURRENT_PREFIX for each environment hook - AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX - # restore IFS before sourcing other files - IFS=$_package_local_setup_IFS - . "$_hook" - done - unset _hook - IFS=$_package_local_setup_IFS - unset _package_local_setup_IFS - unset AMENT_ENVIRONMENT_HOOKS -fi - -unset _package_local_setup_AMENT_CURRENT_PREFIX -unset AMENT_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash deleted file mode 100644 index 27a9ab68..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.bash +++ /dev/null @@ -1,39 +0,0 @@ -# generated from colcon_bash/shell/template/package.bash.em - -# This script extends the environment for this package. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - # the prefix is two levels up from the package specific share directory - _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)" -else - _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source sh script of this package -_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/package.sh" - -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts -COLCON_CURRENT_PREFIX="$_colcon_package_bash_COLCON_CURRENT_PREFIX" - -# source bash hooks -_colcon_package_bash_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/local_setup.bash" - -unset COLCON_CURRENT_PREFIX - -unset _colcon_package_bash_source_script -unset _colcon_package_bash_COLCON_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv deleted file mode 100644 index bf76ceda..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.dsv +++ /dev/null @@ -1,8 +0,0 @@ -source;share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1 -source;share/crane_plus_moveit_config/hook/cmake_prefix_path.dsv -source;share/crane_plus_moveit_config/hook/cmake_prefix_path.sh -source;share/crane_plus_moveit_config/local_setup.bash -source;share/crane_plus_moveit_config/local_setup.dsv -source;share/crane_plus_moveit_config/local_setup.ps1 -source;share/crane_plus_moveit_config/local_setup.sh -source;share/crane_plus_moveit_config/local_setup.zsh diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 deleted file mode 100644 index 6e321cf1..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.ps1 +++ /dev/null @@ -1,116 +0,0 @@ -# generated from colcon_powershell/shell/template/package.ps1.em - -# function to append a value to a variable -# which uses colons as separators -# duplicates as well as leading separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -function colcon_append_unique_value { - param ( - $_listname, - $_value - ) - - # get values from variable - if (Test-Path Env:$_listname) { - $_values=(Get-Item env:$_listname).Value - } else { - $_values="" - } - $_duplicate="" - # start with no values - $_all_values="" - # iterate over existing values in the variable - if ($_values) { - $_values.Split(";") | ForEach { - # not an empty string - if ($_) { - # not a duplicate of _value - if ($_ -eq $_value) { - $_duplicate="1" - } - if ($_all_values) { - $_all_values="${_all_values};$_" - } else { - $_all_values="$_" - } - } - } - } - # append only non-duplicates - if (!$_duplicate) { - # avoid leading separator - if ($_all_values) { - $_all_values="${_all_values};${_value}" - } else { - $_all_values="${_value}" - } - } - - # export the updated variable - Set-Item env:\$_listname -Value "$_all_values" -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -function colcon_prepend_unique_value { - param ( - $_listname, - $_value - ) - - # get values from variable - if (Test-Path Env:$_listname) { - $_values=(Get-Item env:$_listname).Value - } else { - $_values="" - } - # start with the new value - $_all_values="$_value" - # iterate over existing values in the variable - if ($_values) { - $_values.Split(";") | ForEach { - # not an empty string - if ($_) { - # not a duplicate of _value - if ($_ -ne $_value) { - # keep non-duplicate values - $_all_values="${_all_values};$_" - } - } - } - } - # export the updated variable - Set-Item env:\$_listname -Value "$_all_values" -} - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -function colcon_package_source_powershell_script { - param ( - $_colcon_package_source_powershell_script - ) - # source script with conditional trace output - if (Test-Path $_colcon_package_source_powershell_script) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_package_source_powershell_script'" - } - . "$_colcon_package_source_powershell_script" - } else { - Write-Error "not found: '$_colcon_package_source_powershell_script'" - } -} - - -# a powershell script is able to determine its own path -# the prefix is two levels up from the package specific share directory -$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName - -colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/crane_plus_moveit_config/hook/cmake_prefix_path.ps1" -colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/crane_plus_moveit_config/local_setup.ps1" - -Remove-Item Env:\COLCON_CURRENT_PREFIX diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh deleted file mode 100644 index 57bb4350..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.sh +++ /dev/null @@ -1,87 +0,0 @@ -# generated from colcon_core/shell/template/package.sh.em - -# This script extends the environment for this package. - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prepend_unique_value_IFS=$IFS - IFS=":" - # start with the new value - _all_values="$_value" - # workaround SH_WORD_SPLIT not being set in zsh - if [ "$(command -v colcon_zsh_convert_to_array)" ]; then - colcon_zsh_convert_to_array _values - fi - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - # restore the field separator - IFS=$_colcon_prepend_unique_value_IFS - unset _colcon_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/user/overlay_ws/src/crane_plus/install/crane_plus_moveit_config" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_package_sh_COLCON_CURRENT_PREFIX - return 1 - fi - COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX" -fi -unset _colcon_package_sh_COLCON_CURRENT_PREFIX - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source sh hooks -_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/hook/cmake_prefix_path.sh" -_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/local_setup.sh" - -unset _colcon_package_sh_source_script -unset COLCON_CURRENT_PREFIX - -# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml deleted file mode 100644 index 108dd2cf..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.xml +++ /dev/null @@ -1,51 +0,0 @@ - - - - crane_plus_moveit_config - 0.3.0 - - An automatically generated package with all the configuration and launch files for using the crane_plus with the MoveIt Motion Planning Framework - - RT Corporation - - BSD-3-Clause - - http://moveit.ros.org/ - https://github.com/moveit/moveit2/issues - https://github.com/moveit/moveit2 - - RT Corporation - - ament_cmake - - moveit_ros_move_group - moveit_kinematics - moveit_planners - moveit_simple_controller_manager - joint_state_publisher - joint_state_publisher_gui - tf2_ros - xacro - - - - controller_manager - crane_plus_description - moveit_configs_utils - moveit_ros_move_group - moveit_ros_visualization - moveit_ros_warehouse - moveit_setup_assistant - robot_state_publisher - rviz2 - rviz_common - rviz_default_plugins - tf2_ros - warehouse_ros_mongo - - - - ament_cmake - - diff --git a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh b/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh deleted file mode 100644 index 8e6006dc..00000000 --- a/install/crane_plus_moveit_config/share/crane_plus_moveit_config/package.zsh +++ /dev/null @@ -1,50 +0,0 @@ -# generated from colcon_zsh/shell/template/package.zsh.em - -# This script extends the environment for this package. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - # the prefix is two levels up from the package specific share directory - _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)" -else - _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -# additional arguments: arguments to the script -_colcon_package_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$@" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -colcon_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# source sh script of this package -_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/package.sh" -unset convert_zsh_to_array - -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced scripts -COLCON_CURRENT_PREFIX="$_colcon_package_zsh_COLCON_CURRENT_PREFIX" - -# source zsh hooks -_colcon_package_zsh_source_script "$COLCON_CURRENT_PREFIX/share/crane_plus_moveit_config/local_setup.zsh" - -unset COLCON_CURRENT_PREFIX - -unset _colcon_package_zsh_source_script -unset _colcon_package_zsh_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.bash b/install/local_setup.bash deleted file mode 100644 index 03f00256..00000000 --- a/install/local_setup.bash +++ /dev/null @@ -1,121 +0,0 @@ -# generated from colcon_bash/shell/template/prefix.bash.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a bash script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -else - _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_bash_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" - unset _colcon_prefix_bash_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_bash_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.ps1 b/install/local_setup.ps1 deleted file mode 100644 index 6f68c8de..00000000 --- a/install/local_setup.ps1 +++ /dev/null @@ -1,55 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix.ps1.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# check environment variable for custom Python executable -if ($env:COLCON_PYTHON_EXECUTABLE) { - if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { - echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" - exit 1 - } - $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" -} else { - # use the Python executable known at configure time - $_colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { - if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { - echo "error: unable to find python3 executable" - exit 1 - } - $_colcon_python_executable="python3" - } -} - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_powershell_source_script { - param ( - $_colcon_prefix_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_powershell_source_script_param'" - } - . "$_colcon_prefix_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" - } -} - -# get all commands in topological order -$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 - -# execute all commands in topological order -if ($env:COLCON_TRACE) { - echo "Execute generated script:" - echo "<<<" - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output - echo ">>>" -} -if ($_colcon_ordered_commands) { - $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression -} diff --git a/install/local_setup.sh b/install/local_setup.sh deleted file mode 100644 index 78e2b408..00000000 --- a/install/local_setup.sh +++ /dev/null @@ -1,137 +0,0 @@ -# generated from colcon_core/shell/template/prefix.sh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/user/overlay_ws/src/crane_plus/install" -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX - return 1 - fi -else - _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_sh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" - unset _colcon_prefix_sh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_sh_prepend_unique_value - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "_colcon_prefix_sh_source_script() { - if [ -f \"\$1\" ]; then - if [ -n \"\$COLCON_TRACE\" ]; then - echo \"# . \\\"\$1\\\"\" - fi - . \"\$1\" - else - echo \"not found: \\\"\$1\\\"\" 1>&2 - fi - }" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.zsh b/install/local_setup.zsh deleted file mode 100644 index b6487102..00000000 --- a/install/local_setup.zsh +++ /dev/null @@ -1,134 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix.zsh.em - -# This script extends the environment with all packages contained in this -# prefix path. - -# a zsh script is able to determine its own path if necessary -if [ -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -else - _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -fi - -# function to convert array-like strings into arrays -# to workaround SH_WORD_SPLIT not being set -_colcon_prefix_zsh_convert_to_array() { - local _listname=$1 - local _dollar="$" - local _split="{=" - local _to_array="(\"$_dollar$_split$_listname}\")" - eval $_listname=$_to_array -} - -# function to prepend a value to a variable -# which uses colons as separators -# duplicates as well as trailing separators are avoided -# first argument: the name of the result variable -# second argument: the value to be prepended -_colcon_prefix_zsh_prepend_unique_value() { - # arguments - _listname="$1" - _value="$2" - - # get values from variable - eval _values=\"\$$_listname\" - # backup the field separator - _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" - IFS=":" - # start with the new value - _all_values="$_value" - _contained_value="" - # workaround SH_WORD_SPLIT not being set - _colcon_prefix_zsh_convert_to_array _values - # iterate over existing values in the variable - for _item in $_values; do - # ignore empty strings - if [ -z "$_item" ]; then - continue - fi - # ignore duplicates of _value - if [ "$_item" = "$_value" ]; then - _contained_value=1 - continue - fi - # keep non-duplicate values - _all_values="$_all_values:$_item" - done - unset _item - if [ -z "$_contained_value" ]; then - if [ -n "$COLCON_TRACE" ]; then - if [ "$_all_values" = "$_value" ]; then - echo "export $_listname=$_value" - else - echo "export $_listname=$_value:\$$_listname" - fi - fi - fi - unset _contained_value - # restore the field separator - IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" - unset _colcon_prefix_zsh_prepend_unique_value_IFS - # export the updated variable - eval export $_listname=\"$_all_values\" - unset _all_values - unset _values - - unset _value - unset _listname -} - -# add this prefix to the COLCON_PREFIX_PATH -_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" -unset _colcon_prefix_zsh_prepend_unique_value -unset _colcon_prefix_zsh_convert_to_array - -# check environment variable for custom Python executable -if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then - if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then - echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" - return 1 - fi - _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" -else - # try the Python executable known at configure time - _colcon_python_executable="/usr/bin/python3" - # if it doesn't exist try a fall back - if [ ! -f "$_colcon_python_executable" ]; then - if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then - echo "error: unable to find python3 executable" - return 1 - fi - _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` - fi -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# get all commands in topological order -_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" -unset _colcon_python_executable -if [ -n "$COLCON_TRACE" ]; then - echo "$(declare -f _colcon_prefix_sh_source_script)" - echo "# Execute generated script:" - echo "# <<<" - echo "${_colcon_ordered_commands}" - echo "# >>>" - echo "unset _colcon_prefix_sh_source_script" -fi -eval "${_colcon_ordered_commands}" -unset _colcon_ordered_commands - -unset _colcon_prefix_sh_source_script - -unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/install/setup.bash b/install/setup.bash deleted file mode 100644 index 07a556ce..00000000 --- a/install/setup.bash +++ /dev/null @@ -1,34 +0,0 @@ -# generated from colcon_bash/shell/template/prefix_chain.bash.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_bash_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/jazzy" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/user/overlay_ws/install" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" -_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.ps1 b/install/setup.ps1 deleted file mode 100644 index 2d91df92..00000000 --- a/install/setup.ps1 +++ /dev/null @@ -1,30 +0,0 @@ -# generated from colcon_powershell/shell/template/prefix_chain.ps1.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -function _colcon_prefix_chain_powershell_source_script { - param ( - $_colcon_prefix_chain_powershell_source_script_param - ) - # source script with conditional trace output - if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { - if ($env:COLCON_TRACE) { - echo ". '$_colcon_prefix_chain_powershell_source_script_param'" - } - . "$_colcon_prefix_chain_powershell_source_script_param" - } else { - Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" - } -} - -# source chained prefixes -_colcon_prefix_chain_powershell_source_script "/opt/ros/jazzy\local_setup.ps1" -_colcon_prefix_chain_powershell_source_script "/home/user/overlay_ws/install\local_setup.ps1" - -# source this prefix -$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) -_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/install/setup.sh b/install/setup.sh deleted file mode 100644 index 78792771..00000000 --- a/install/setup.sh +++ /dev/null @@ -1,49 +0,0 @@ -# generated from colcon_core/shell/template/prefix_chain.sh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# since a plain shell script can't determine its own path when being sourced -# either use the provided COLCON_CURRENT_PREFIX -# or fall back to the build time prefix (if it exists) -_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/user/overlay_ws/src/crane_plus/install -if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then - _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" -elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then - echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 - unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX - return 1 -fi - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_sh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/jazzy" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="/home/user/overlay_ws/install" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script -COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" -_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" - -unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_sh_source_script -unset COLCON_CURRENT_PREFIX diff --git a/install/setup.zsh b/install/setup.zsh deleted file mode 100644 index 99962a2a..00000000 --- a/install/setup.zsh +++ /dev/null @@ -1,34 +0,0 @@ -# generated from colcon_zsh/shell/template/prefix_chain.zsh.em - -# This script extends the environment with the environment of other prefix -# paths which were sourced when this file was generated as well as all packages -# contained in this prefix path. - -# function to source another script with conditional trace output -# first argument: the path of the script -_colcon_prefix_chain_zsh_source_script() { - if [ -f "$1" ]; then - if [ -n "$COLCON_TRACE" ]; then - echo "# . \"$1\"" - fi - . "$1" - else - echo "not found: \"$1\"" 1>&2 - fi -} - -# source chained prefixes -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/opt/ros/jazzy" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="/home/user/overlay_ws/install" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -# source this prefix -# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script -COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" -_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" - -unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE deleted file mode 100644 index e69de29b..00000000 diff --git a/log/latest b/log/latest deleted file mode 120000 index b57d247c..00000000 --- a/log/latest +++ /dev/null @@ -1 +0,0 @@ -latest_build \ No newline at end of file diff --git a/log/latest_build b/log/latest_build deleted file mode 120000 index 2552eddc..00000000 --- a/log/latest_build +++ /dev/null @@ -1 +0,0 @@ -build_2024-07-24_16-33-58 \ No newline at end of file From 89bdf8bee350a57c33853a64b430158667c5555e Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Tue, 13 Aug 2024 12:40:44 +0900 Subject: [PATCH 004/111] =?UTF-8?q?crane=5Fplus=5Fmoveit=5Fconfig/launch?= =?UTF-8?q?=E3=81=AE=E4=B8=AD=E3=81=AE=E4=B8=8D=E8=A6=81=E3=83=95=E3=82=A1?= =?UTF-8?q?=E3=82=A4=E3=83=AB=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 10 +++++++++- crane_plus_moveit_config/launch/demo.launch.py | 7 ------- crane_plus_moveit_config/launch/move_group.launch.py | 7 ------- crane_plus_moveit_config/launch/moveit_rviz.launch.py | 7 ------- crane_plus_moveit_config/launch/rsp.launch.py | 7 ------- .../launch/setup_assistant.launch.py | 7 ------- .../launch/spawn_controllers.launch.py | 7 ------- .../launch/static_virtual_joint_tfs.launch.py | 7 ------- crane_plus_moveit_config/launch/warehouse_db.launch.py | 7 ------- 9 files changed, 9 insertions(+), 57 deletions(-) delete mode 100755 crane_plus_moveit_config/launch/demo.launch.py delete mode 100755 crane_plus_moveit_config/launch/move_group.launch.py delete mode 100755 crane_plus_moveit_config/launch/moveit_rviz.launch.py delete mode 100755 crane_plus_moveit_config/launch/rsp.launch.py delete mode 100755 crane_plus_moveit_config/launch/setup_assistant.launch.py delete mode 100755 crane_plus_moveit_config/launch/spawn_controllers.launch.py delete mode 100755 crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py delete mode 100755 crane_plus_moveit_config/launch/warehouse_db.launch.py diff --git a/.gitignore b/.gitignore index 6d611533..36a17eb9 100755 --- a/.gitignore +++ b/.gitignore @@ -61,6 +61,14 @@ target/ # unnecessary crane_plus_moveit_config/.setup_assistant +crane_plus_moveit_config/launch/demo.launch.py +crane_plus_moveit_config/launch/move_group.launch.py +crane_plus_moveit_config/launch/rsp.launch.py +crane_plus_moveit_config/launch/setup_assistant.launch.py +crane_plus_moveit_config/launch/spawn_controllers.launch.py +crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py +crane_plus_moveit_config/launch/warehouse_db.launch.py +crane_plus_moveit_config/launch/moveit_rviz.launch.py install/ log/ -crane_plus_examples/scripts/ +crane_plus_examples/scripts diff --git a/crane_plus_moveit_config/launch/demo.launch.py b/crane_plus_moveit_config/launch/demo.launch.py deleted file mode 100755 index 2b134ee0..00000000 --- a/crane_plus_moveit_config/launch/demo.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_demo_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_demo_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/move_group.launch.py b/crane_plus_moveit_config/launch/move_group.launch.py deleted file mode 100755 index d05ee6c5..00000000 --- a/crane_plus_moveit_config/launch/move_group.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_move_group_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_move_group_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/moveit_rviz.launch.py b/crane_plus_moveit_config/launch/moveit_rviz.launch.py deleted file mode 100755 index 9034635d..00000000 --- a/crane_plus_moveit_config/launch/moveit_rviz.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_moveit_rviz_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_moveit_rviz_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/rsp.launch.py b/crane_plus_moveit_config/launch/rsp.launch.py deleted file mode 100755 index 28439b32..00000000 --- a/crane_plus_moveit_config/launch/rsp.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_rsp_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_rsp_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py deleted file mode 100755 index e16485a5..00000000 --- a/crane_plus_moveit_config/launch/setup_assistant.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_setup_assistant_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_setup_assistant_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/spawn_controllers.launch.py b/crane_plus_moveit_config/launch/spawn_controllers.launch.py deleted file mode 100755 index 85a8dfc7..00000000 --- a/crane_plus_moveit_config/launch/spawn_controllers.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_spawn_controllers_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_spawn_controllers_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py b/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py deleted file mode 100755 index 7ff90d29..00000000 --- a/crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/crane_plus_moveit_config/launch/warehouse_db.launch.py b/crane_plus_moveit_config/launch/warehouse_db.launch.py deleted file mode 100755 index 92981372..00000000 --- a/crane_plus_moveit_config/launch/warehouse_db.launch.py +++ /dev/null @@ -1,7 +0,0 @@ -from moveit_configs_utils import MoveItConfigsBuilder -from moveit_configs_utils.launches import generate_warehouse_db_launch - - -def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() - return generate_warehouse_db_launch(moveit_config) From 96186f6938ca26495de608cf72a06be35fa237d8 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 10:16:27 +0900 Subject: [PATCH 005/111] =?UTF-8?q?default=5F***=5Fscaling=5Ffactor?= =?UTF-8?q?=E3=82=920.1->1.0=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/config/joint_limits.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_plus_moveit_config/config/joint_limits.yaml b/crane_plus_moveit_config/config/joint_limits.yaml index 45544801..20737bce 100755 --- a/crane_plus_moveit_config/config/joint_limits.yaml +++ b/crane_plus_moveit_config/config/joint_limits.yaml @@ -2,8 +2,8 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1.0 +default_acceleration_scaling_factor: 1.0 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] From e3898ed5bc34ef72d3746108e66a49e137a9c173 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 11:33:16 +0900 Subject: [PATCH 006/111] =?UTF-8?q?setup=20assistant=E3=81=AB=E3=82=88?= =?UTF-8?q?=E3=82=8B=E6=9B=B8=E6=8F=9B=E3=82=8F=E3=82=8A=E3=82=92=E5=85=83?= =?UTF-8?q?=E3=81=AE=E6=9B=B8=E3=81=8D=E6=96=B9=E3=81=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/CMakeLists.txt | 20 ++++++++++++-------- crane_plus_moveit_config/package.xml | 18 +++++++++--------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/crane_plus_moveit_config/CMakeLists.txt b/crane_plus_moveit_config/CMakeLists.txt index 895a3c1e..20da915b 100755 --- a/crane_plus_moveit_config/CMakeLists.txt +++ b/crane_plus_moveit_config/CMakeLists.txt @@ -1,13 +1,17 @@ -cmake_minimum_required(VERSION 3.22) +cmake_minimum_required(VERSION 3.5) project(crane_plus_moveit_config) find_package(ament_cmake REQUIRED) -ament_package() +install(DIRECTORY + launch + config + DESTINATION share/${PROJECT_NAME}/ +) -install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} - PATTERN "setup_assistant.launch" EXCLUDE) -install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) -install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/crane_plus_moveit_config/package.xml b/crane_plus_moveit_config/package.xml index d4e67418..5e8554ba 100755 --- a/crane_plus_moveit_config/package.xml +++ b/crane_plus_moveit_config/package.xml @@ -2,20 +2,18 @@ crane_plus_moveit_config - 0.3.0 - - An automatically generated package with all the configuration and launch files for using the crane_plus with the MoveIt Motion Planning Framework - + 2.0.1 + CRANE+ V2 move_group config package RT Corporation - - BSD-3-Clause + Apache License 2.0 + Shota Aoki + Atsushi Kuwagata + Nozomi Mizoguchi http://moveit.ros.org/ https://github.com/moveit/moveit2/issues https://github.com/moveit/moveit2 - RT Corporation - ament_cmake moveit_ros_move_group @@ -40,7 +38,9 @@ tf2_ros warehouse_ros_mongo - + ament_lint_auto + ament_lint_common + ament_cmake From 553e522b344d6e94c1ed00cc1ef7ca1be738aae3 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 11:48:51 +0900 Subject: [PATCH 007/111] =?UTF-8?q?example=E3=81=AEdemo.lauch=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples/launch/demo.launch.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 49e90d08..17cd579c 100755 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -30,6 +30,7 @@ def generate_launch_description(): default_value='/dev/ttyUSB0', description='Set port name.' ) + declare_use_camera = DeclareLaunchArgument( 'use_camera', default_value='false', @@ -96,5 +97,7 @@ def generate_launch_description(): declare_use_camera, declare_video_device, move_group, + move_group_camera, control_node, + usb_cam_node ]) From 384f2b49b70fd742e88e7390cc5f0dc46d9b0560 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 13:55:12 +0900 Subject: [PATCH 008/111] =?UTF-8?q?run=5Fmove=5Fgroup.rviz=E5=89=8A?= =?UTF-8?q?=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/run_move_group.rviz | 351 ------------------ 1 file changed, 351 deletions(-) delete mode 100755 crane_plus_moveit_config/launch/run_move_group.rviz diff --git a/crane_plus_moveit_config/launch/run_move_group.rviz b/crane_plus_moveit_config/launch/run_move_group.rviz deleted file mode 100755 index 44b5c6a9..00000000 --- a/crane_plus_moveit_config/launch/run_move_group.rviz +++ /dev/null @@ -1,351 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /MotionPlanning1 - - /Trajectory1 - Splitter Ratio: 0.5 - Tree Height: 295 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Acceleration_Scaling_Factor: 1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Goal_Tolerance: 0 - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: true - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - crane_plus_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: /move_group/display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 1 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0 - Joint Violation Color: 255; 0; 255 - Planning Group: arm - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 1 - Start State Color: 0; 255; 0 - Planning Scene Topic: monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - crane_plus_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 1 - - Class: moveit_rviz_plugin/Trajectory - Color Enabled: false - Enabled: true - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - crane_plus_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - crane_plus_link_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: false - Name: Trajectory - Robot Alpha: 0.5 - Robot Color: 150; 50; 150 - Robot Description: robot_description - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.05 s - Trail Step Size: 1 - Trajectory Topic: display_planned_path - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 0.8449440598487854 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.04136588051915169 - Y: -0.017012163996696472 - Z: 0.08837244659662247 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.44539815187454224 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.5553978681564331 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 1056 - Hide Left Dock: false - Hide Right Dock: false - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 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 - Selection: - collapsed: false - Tool Properties: - collapsed: false - Trajectory - Trajectory Slider: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 From 7147e27211a90f4350125b2c3cca576bc33e4594 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 14:02:53 +0900 Subject: [PATCH 009/111] =?UTF-8?q?sensors=5F3d.yaml=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/sensors_3d.yaml | 24 ------------------- 1 file changed, 24 deletions(-) delete mode 100755 crane_plus_moveit_config/config/sensors_3d.yaml diff --git a/crane_plus_moveit_config/config/sensors_3d.yaml b/crane_plus_moveit_config/config/sensors_3d.yaml deleted file mode 100755 index ab5c1e18..00000000 --- a/crane_plus_moveit_config/config/sensors_3d.yaml +++ /dev/null @@ -1,24 +0,0 @@ -sensors: - - kinect_pointcloud - - kinect_depthimage -kinect_pointcloud: - filtered_cloud_topic: filtered_cloud - max_range: 5.0 - max_update_rate: 1.0 - padding_offset: 0.1 - padding_scale: 1.0 - point_cloud_topic: /head_mount_kinect/depth_registered/points - point_subsample: 1 - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater -kinect_depthimage: - far_clipping_plane_distance: 5.0 - filtered_cloud_topic: filtered_cloud - image_topic: /head_mount_kinect/depth_registered/image_raw - max_update_rate: 1.0 - near_clipping_plane_distance: 0.3 - padding_offset: 0.03 - padding_scale: 4.0 - queue_size: 5 - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - shadow_threshold: 0.2 - \ No newline at end of file From de86b8d600d939983f231aff9550cc9f6b72a217 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 14:09:53 +0900 Subject: [PATCH 010/111] =?UTF-8?q?solver=E3=82=92=E5=85=83=E3=81=AB?= =?UTF-8?q?=E6=88=BB=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/config/kinematics.yaml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/crane_plus_moveit_config/config/kinematics.yaml b/crane_plus_moveit_config/config/kinematics.yaml index c9fd73d3..084a052c 100755 --- a/crane_plus_moveit_config/config/kinematics.yaml +++ b/crane_plus_moveit_config/config/kinematics.yaml @@ -1,6 +1,5 @@ arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_attempts: 10 kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 - \ No newline at end of file + kinematics_solver_timeout: 0.005 From 0314d62217dc86198108e66ec8a6a3c6434bb816 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 14:39:44 +0900 Subject: [PATCH 011/111] =?UTF-8?q?.gitignore=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/.gitignore b/.gitignore index 36a17eb9..04d4c3a6 100755 --- a/.gitignore +++ b/.gitignore @@ -56,19 +56,4 @@ docs/_build/ # PyBuilder target/ .pytest_cache - .vscode - -# unnecessary -crane_plus_moveit_config/.setup_assistant -crane_plus_moveit_config/launch/demo.launch.py -crane_plus_moveit_config/launch/move_group.launch.py -crane_plus_moveit_config/launch/rsp.launch.py -crane_plus_moveit_config/launch/setup_assistant.launch.py -crane_plus_moveit_config/launch/spawn_controllers.launch.py -crane_plus_moveit_config/launch/static_virtual_joint_tfs.launch.py -crane_plus_moveit_config/launch/warehouse_db.launch.py -crane_plus_moveit_config/launch/moveit_rviz.launch.py -install/ -log/ -crane_plus_examples/scripts From 185c6bd36a2364602c0819326c0ae0eb393275c5 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 14:43:03 +0900 Subject: [PATCH 012/111] =?UTF-8?q?setup=5Fassistant=E9=96=A2=E9=80=A3?= =?UTF-8?q?=E3=81=AE=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/.setup_assistant | 25 +++++++++++++++++++ .../launch/setup_assistant.launch.py | 7 ++++++ 2 files changed, 32 insertions(+) create mode 100755 crane_plus_moveit_config/.setup_assistant create mode 100755 crane_plus_moveit_config/launch/setup_assistant.launch.py diff --git a/crane_plus_moveit_config/.setup_assistant b/crane_plus_moveit_config/.setup_assistant new file mode 100755 index 00000000..3058c8a2 --- /dev/null +++ b/crane_plus_moveit_config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: crane_plus_description + relative_path: urdf/crane_plus.urdf.xacro + srdf: + relative_path: config/crane_plus.srdf + package_settings: + author_name: RT Corporation + author_email: shop@rt-net.jp + generated_timestamp: 1721799427 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py new file mode 100755 index 00000000..e16485a5 --- /dev/null +++ b/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) From 2e0161004d0cb2ea3bdbbcc54c3c2e39b7d6334d Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 14:47:36 +0900 Subject: [PATCH 013/111] =?UTF-8?q?ci=E3=81=AEjazzy=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 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index f539e586..d809d96e 100755 --- 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 } + - { ROS_DISTRO: jazzy, ROS_REPO: ros } runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 From 4ba5ed88b306955f014e7611ae8d3414857937da Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 16:25:37 +0900 Subject: [PATCH 014/111] =?UTF-8?q?run=5Fmove=5Ffroup.lauch.py=E3=81=AE?= =?UTF-8?q?=E6=95=B4=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/run_move_group.launch.py | 76 ++++++------------- 1 file changed, 23 insertions(+), 53 deletions(-) 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 fb4e9a8d..e20593ca 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -18,26 +18,13 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node import yaml 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.launch_utils import ( - add_debuggable_node, - DeclareBooleanLaunchArg, -) - - -# Reference: https://github.com/ros-planning/moveit2/blob/main/moveit_demo_nodes/ -# run_move_group/launch/run_move_group.launch.py - +from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg def load_file(package_name, file_path): package_path = get_package_share_directory(package_name) @@ -77,11 +64,23 @@ def generate_launch_description(): ld.add_action(declare_loaded_description) - # MoveItConfigBuilderによるパラメータ設定 + ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) + + ld.add_action( + DeclareLaunchArgument( + "rviz_config", + default_value=get_package_share_directory("crane_plus_moveit_config") + + "/config/moveit.rviz", + description="Set the path to rviz configuration file.", + ) + ) + moveit_config = ( - # ロボット名の定義 MoveItConfigsBuilder("crane_plus") - # URDFの設定 + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) .robot_description( file_path=os.path.join( get_package_share_directory("crane_plus_description"), @@ -90,29 +89,15 @@ def generate_launch_description(): ), mappings={}, ) - # SRDFの設定 .robot_description_semantic( file_path="config/crane_plus.srdf", mappings={"model": "crane_plus"}, ) .joint_limits(file_path="config/joint_limits.yaml") - # Planning Sceneのトピックの設定 - .planning_scene_monitor( - publish_planning_scene=True, - publish_geometry_updates=True, - publish_state_updates=True, - publish_transforms_updates=True, - publish_robot_description=True, - publish_robot_description_semantic=True, - ) - # 軌道追従制御ノードの設定 .trajectory_execution( - file_path="config/controllers.yaml", - moveit_manage_controllers=True + file_path="config/controllers.yaml", moveit_manage_controllers=True ) - # 軌道計画のプラグイン設定 - .planning_pipelines(pipelines=["ompl"], default_planning_pipeline="ompl") - # キネマティクスの設定 + .planning_pipelines(pipelines=["ompl"]) .robot_description_kinematics(file_path="config/kinematics.yaml") .to_moveit_configs() ) @@ -121,33 +106,18 @@ def generate_launch_description(): "robot_description": LaunchConfiguration("loaded_description") } - moveit_config.move_group_capabilities = {"capabilities": ""} + moveit_config.move_group_capabilities = { + "capabilities": "" + } # Move group ld.add_entity(generate_move_group_launch(moveit_config)) # RViz ld.add_entity(generate_moveit_rviz_launch(moveit_config)) - ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) - ld.add_action( - DeclareLaunchArgument( - "rviz_config", - default_value=get_package_share_directory("crane_plus_moveit_config") - + "/config/moveit.rviz", - description="Set the path to rviz configuration file.", - ) - ) - + # Static TF - ld.add_action( - Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], - ) - ) + ld.add_entity(generate_static_virtual_joint_tfs_launch(moveit_config)) # Publish TF ld.add_entity(generate_rsp_launch(moveit_config)) From 92f9340b576988bf84dbc8cb0a7e968561ac542f Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 17:50:00 +0900 Subject: [PATCH 015/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E6=A8=A9?= =?UTF-8?q?=E9=99=90=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .docker/README.md | 0 .docker/source/Dockerfile | 0 .dockerignore | 0 .github/workflows/build_docker_image.yml | 0 LICENSE | 0 README.md | 0 crane_plus/CHANGELOG.rst | 0 crane_plus/CMakeLists.txt | 0 crane_plus/package.xml | 0 crane_plus_control/CMakeLists.txt | 0 crane_plus_control/README.md | 0 .../config/crane_plus_controllers.yaml | 0 crane_plus_control/crane_plus_hardware.xml | 0 .../crane_plus_control/crane_plus_driver.hpp | 0 .../crane_plus_hardware.hpp | 0 .../crane_plus_control/visibility_control.h | 0 .../launch/crane_plus_control.launch.py | 0 crane_plus_control/package.xml | 0 crane_plus_control/src/crane_plus_driver.cpp | 0 .../src/crane_plus_hardware.cpp | 0 crane_plus_description/CHANGELOG.rst | 0 crane_plus_description/CMakeLists.txt | 0 crane_plus_description/README.md | 0 .../crane_plus_description/__init__.py | 0 .../launch/display.launch.py | 0 crane_plus_description/launch/display.rviz | 0 .../collision/Hand_collision_20201013.stl | Bin .../collision/Link1_collision_20201013.stl | Bin .../collision/Link2_collision_20201013.stl | Bin .../collision/Link3_collision_20201013.stl | Bin .../collision/Link4_collision_20201013.stl | Bin .../collision/body_collision_20201013.stl | Bin .../meshes/visual/Hand_20201013.stl | Bin .../meshes/visual/Link1_20201013.stl | Bin .../meshes/visual/Link2_20201013.stl | Bin .../meshes/visual/Link3_20201013.stl | Bin .../meshes/visual/Link4_20201013.stl | Bin .../meshes/visual/body_20201013.stl | Bin .../test/dummy_controllers.yaml | 0 .../test/test_robot_description_loader.py | 0 crane_plus_description/urdf/camera.urdf.xacro | 0 .../urdf/camera_stand.urdf.xacro | 0 .../urdf/crane_plus.gazebo.xacro | 0 .../urdf/crane_plus.gazebo_ros2_control.xacro | 0 .../urdf/crane_plus.ros2_control.xacro | 0 .../urdf/crane_plus.urdf.xacro | 0 crane_plus_description/urdf/crane_plus.xacro | 0 crane_plus_examples/CHANGELOG.rst | 0 crane_plus_examples/CMakeLists.txt | 0 crane_plus_examples/README.md | 0 crane_plus_examples/aruco_markers.pdf | Bin crane_plus_examples/config/camera_info.yaml | 0 .../launch/camera_example.launch.py | 0 .../launch/camera_example.rviz | 0 crane_plus_examples/launch/demo.launch.py | 0 crane_plus_examples/launch/example.launch.py | 0 crane_plus_examples/package.xml | 0 .../scripts/aruco_detection.py | 51 ++++++++++++++++++ .../scripts/color_detection.py | 14 +++++ .../scripts/gripper_control.py | 48 +++++++++++++++++ crane_plus_examples/scripts/joint_values.py | 14 +++++ crane_plus_examples/scripts/pick_and_place.py | 14 +++++ .../scripts/pick_and_place_tf.py | 14 +++++ .../scripts/pose_groupstate.py | 14 +++++ crane_plus_examples/src/aruco_detection.cpp | 0 crane_plus_examples/src/color_detection.cpp | 0 crane_plus_examples/src/gripper_control.cpp | 0 crane_plus_examples/src/joint_values.cpp | 0 crane_plus_examples/src/pick_and_place.cpp | 0 crane_plus_examples/src/pick_and_place_tf.cpp | 0 crane_plus_examples/src/pose_groupstate.cpp | 0 crane_plus_gazebo/CHANGELOG.rst | 0 crane_plus_gazebo/CMakeLists.txt | 0 crane_plus_gazebo/README.md | 0 crane_plus_gazebo/gui/gui.config | 0 .../launch/crane_plus_with_table.launch.py | 0 crane_plus_gazebo/package.xml | 0 crane_plus_gazebo/worlds/table.sdf | 0 crane_plus_moveit_config/.setup_assistant | 0 crane_plus_moveit_config/CHANGELOG.rst | 0 crane_plus_moveit_config/CMakeLists.txt | 0 crane_plus_moveit_config/README.md | 0 .../config/controllers.yaml | 0 .../config/crane_plus.srdf | 0 .../config/joint_limits.yaml | 0 .../config/kinematics.yaml | 11 ++-- crane_plus_moveit_config/config/moveit.rviz | 0 .../config/ompl_planning.yaml | 0 .../config/pilz_cartesian_limits.yaml | 0 89 files changed, 177 insertions(+), 3 deletions(-) mode change 100755 => 100644 .docker/README.md mode change 100755 => 100644 .docker/source/Dockerfile mode change 100755 => 100644 .dockerignore mode change 100755 => 100644 .github/workflows/build_docker_image.yml mode change 100755 => 100644 LICENSE mode change 100755 => 100644 README.md mode change 100755 => 100644 crane_plus/CHANGELOG.rst mode change 100755 => 100644 crane_plus/CMakeLists.txt mode change 100755 => 100644 crane_plus/package.xml mode change 100755 => 100644 crane_plus_control/CMakeLists.txt mode change 100755 => 100644 crane_plus_control/README.md mode change 100755 => 100644 crane_plus_control/config/crane_plus_controllers.yaml mode change 100755 => 100644 crane_plus_control/crane_plus_hardware.xml mode change 100755 => 100644 crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp mode change 100755 => 100644 crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp mode change 100755 => 100644 crane_plus_control/include/crane_plus_control/visibility_control.h mode change 100755 => 100644 crane_plus_control/launch/crane_plus_control.launch.py mode change 100755 => 100644 crane_plus_control/package.xml mode change 100755 => 100644 crane_plus_control/src/crane_plus_driver.cpp mode change 100755 => 100644 crane_plus_control/src/crane_plus_hardware.cpp mode change 100755 => 100644 crane_plus_description/CHANGELOG.rst mode change 100755 => 100644 crane_plus_description/CMakeLists.txt mode change 100755 => 100644 crane_plus_description/README.md mode change 100755 => 100644 crane_plus_description/crane_plus_description/__init__.py mode change 100755 => 100644 crane_plus_description/launch/display.launch.py mode change 100755 => 100644 crane_plus_description/launch/display.rviz mode change 100755 => 100644 crane_plus_description/meshes/collision/Hand_collision_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/collision/Link1_collision_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/collision/Link2_collision_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/collision/Link3_collision_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/collision/Link4_collision_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/collision/body_collision_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/visual/Hand_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/visual/Link1_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/visual/Link2_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/visual/Link3_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/visual/Link4_20201013.stl mode change 100755 => 100644 crane_plus_description/meshes/visual/body_20201013.stl mode change 100755 => 100644 crane_plus_description/test/dummy_controllers.yaml mode change 100755 => 100644 crane_plus_description/test/test_robot_description_loader.py mode change 100755 => 100644 crane_plus_description/urdf/camera.urdf.xacro mode change 100755 => 100644 crane_plus_description/urdf/camera_stand.urdf.xacro mode change 100755 => 100644 crane_plus_description/urdf/crane_plus.gazebo.xacro mode change 100755 => 100644 crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro mode change 100755 => 100644 crane_plus_description/urdf/crane_plus.ros2_control.xacro mode change 100755 => 100644 crane_plus_description/urdf/crane_plus.urdf.xacro mode change 100755 => 100644 crane_plus_description/urdf/crane_plus.xacro mode change 100755 => 100644 crane_plus_examples/CHANGELOG.rst mode change 100755 => 100644 crane_plus_examples/CMakeLists.txt mode change 100755 => 100644 crane_plus_examples/README.md mode change 100755 => 100644 crane_plus_examples/aruco_markers.pdf mode change 100755 => 100644 crane_plus_examples/config/camera_info.yaml mode change 100755 => 100644 crane_plus_examples/launch/camera_example.launch.py mode change 100755 => 100644 crane_plus_examples/launch/camera_example.rviz mode change 100755 => 100644 crane_plus_examples/launch/demo.launch.py mode change 100755 => 100644 crane_plus_examples/launch/example.launch.py mode change 100755 => 100644 crane_plus_examples/package.xml create mode 100644 crane_plus_examples/scripts/aruco_detection.py create mode 100644 crane_plus_examples/scripts/color_detection.py create mode 100644 crane_plus_examples/scripts/gripper_control.py create mode 100644 crane_plus_examples/scripts/joint_values.py create mode 100644 crane_plus_examples/scripts/pick_and_place.py create mode 100644 crane_plus_examples/scripts/pick_and_place_tf.py create mode 100644 crane_plus_examples/scripts/pose_groupstate.py mode change 100755 => 100644 crane_plus_examples/src/aruco_detection.cpp mode change 100755 => 100644 crane_plus_examples/src/color_detection.cpp mode change 100755 => 100644 crane_plus_examples/src/gripper_control.cpp mode change 100755 => 100644 crane_plus_examples/src/joint_values.cpp mode change 100755 => 100644 crane_plus_examples/src/pick_and_place.cpp mode change 100755 => 100644 crane_plus_examples/src/pick_and_place_tf.cpp mode change 100755 => 100644 crane_plus_examples/src/pose_groupstate.cpp mode change 100755 => 100644 crane_plus_gazebo/CHANGELOG.rst mode change 100755 => 100644 crane_plus_gazebo/CMakeLists.txt mode change 100755 => 100644 crane_plus_gazebo/README.md mode change 100755 => 100644 crane_plus_gazebo/gui/gui.config mode change 100755 => 100644 crane_plus_gazebo/launch/crane_plus_with_table.launch.py mode change 100755 => 100644 crane_plus_gazebo/package.xml mode change 100755 => 100644 crane_plus_gazebo/worlds/table.sdf mode change 100755 => 100644 crane_plus_moveit_config/.setup_assistant mode change 100755 => 100644 crane_plus_moveit_config/CHANGELOG.rst mode change 100755 => 100644 crane_plus_moveit_config/CMakeLists.txt mode change 100755 => 100644 crane_plus_moveit_config/README.md mode change 100755 => 100644 crane_plus_moveit_config/config/controllers.yaml mode change 100755 => 100644 crane_plus_moveit_config/config/crane_plus.srdf mode change 100755 => 100644 crane_plus_moveit_config/config/joint_limits.yaml mode change 100755 => 100644 crane_plus_moveit_config/config/kinematics.yaml mode change 100755 => 100644 crane_plus_moveit_config/config/moveit.rviz mode change 100755 => 100644 crane_plus_moveit_config/config/ompl_planning.yaml mode change 100755 => 100644 crane_plus_moveit_config/config/pilz_cartesian_limits.yaml diff --git a/.docker/README.md b/.docker/README.md old mode 100755 new mode 100644 diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile old mode 100755 new mode 100644 diff --git a/.dockerignore b/.dockerignore old mode 100755 new mode 100644 diff --git a/.github/workflows/build_docker_image.yml b/.github/workflows/build_docker_image.yml old mode 100755 new mode 100644 diff --git a/LICENSE b/LICENSE old mode 100755 new mode 100644 diff --git a/README.md b/README.md old mode 100755 new mode 100644 diff --git a/crane_plus/CHANGELOG.rst b/crane_plus/CHANGELOG.rst old mode 100755 new mode 100644 diff --git a/crane_plus/CMakeLists.txt b/crane_plus/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/crane_plus/package.xml b/crane_plus/package.xml old mode 100755 new mode 100644 diff --git a/crane_plus_control/CMakeLists.txt b/crane_plus_control/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/crane_plus_control/README.md b/crane_plus_control/README.md old mode 100755 new mode 100644 diff --git a/crane_plus_control/config/crane_plus_controllers.yaml b/crane_plus_control/config/crane_plus_controllers.yaml old mode 100755 new mode 100644 diff --git a/crane_plus_control/crane_plus_hardware.xml b/crane_plus_control/crane_plus_hardware.xml old mode 100755 new mode 100644 diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp old mode 100755 new mode 100644 diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp old mode 100755 new mode 100644 diff --git a/crane_plus_control/include/crane_plus_control/visibility_control.h b/crane_plus_control/include/crane_plus_control/visibility_control.h old mode 100755 new mode 100644 diff --git a/crane_plus_control/launch/crane_plus_control.launch.py b/crane_plus_control/launch/crane_plus_control.launch.py old mode 100755 new mode 100644 diff --git a/crane_plus_control/package.xml b/crane_plus_control/package.xml old mode 100755 new mode 100644 diff --git a/crane_plus_control/src/crane_plus_driver.cpp b/crane_plus_control/src/crane_plus_driver.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_description/CHANGELOG.rst b/crane_plus_description/CHANGELOG.rst old mode 100755 new mode 100644 diff --git a/crane_plus_description/CMakeLists.txt b/crane_plus_description/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/crane_plus_description/README.md b/crane_plus_description/README.md old mode 100755 new mode 100644 diff --git a/crane_plus_description/crane_plus_description/__init__.py b/crane_plus_description/crane_plus_description/__init__.py old mode 100755 new mode 100644 diff --git a/crane_plus_description/launch/display.launch.py b/crane_plus_description/launch/display.launch.py old mode 100755 new mode 100644 diff --git a/crane_plus_description/launch/display.rviz b/crane_plus_description/launch/display.rviz old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/collision/Hand_collision_20201013.stl b/crane_plus_description/meshes/collision/Hand_collision_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/collision/Link1_collision_20201013.stl b/crane_plus_description/meshes/collision/Link1_collision_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/collision/Link2_collision_20201013.stl b/crane_plus_description/meshes/collision/Link2_collision_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/collision/Link3_collision_20201013.stl b/crane_plus_description/meshes/collision/Link3_collision_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/collision/Link4_collision_20201013.stl b/crane_plus_description/meshes/collision/Link4_collision_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/collision/body_collision_20201013.stl b/crane_plus_description/meshes/collision/body_collision_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/visual/Hand_20201013.stl b/crane_plus_description/meshes/visual/Hand_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/visual/Link1_20201013.stl b/crane_plus_description/meshes/visual/Link1_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/visual/Link2_20201013.stl b/crane_plus_description/meshes/visual/Link2_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/visual/Link3_20201013.stl b/crane_plus_description/meshes/visual/Link3_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/visual/Link4_20201013.stl b/crane_plus_description/meshes/visual/Link4_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/meshes/visual/body_20201013.stl b/crane_plus_description/meshes/visual/body_20201013.stl old mode 100755 new mode 100644 diff --git a/crane_plus_description/test/dummy_controllers.yaml b/crane_plus_description/test/dummy_controllers.yaml old mode 100755 new mode 100644 diff --git a/crane_plus_description/test/test_robot_description_loader.py b/crane_plus_description/test/test_robot_description_loader.py old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/camera.urdf.xacro b/crane_plus_description/urdf/camera.urdf.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/camera_stand.urdf.xacro b/crane_plus_description/urdf/camera_stand.urdf.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/crane_plus.gazebo.xacro b/crane_plus_description/urdf/crane_plus.gazebo.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro b/crane_plus_description/urdf/crane_plus.gazebo_ros2_control.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/crane_plus.ros2_control.xacro b/crane_plus_description/urdf/crane_plus.ros2_control.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/crane_plus.urdf.xacro b/crane_plus_description/urdf/crane_plus.urdf.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_description/urdf/crane_plus.xacro b/crane_plus_description/urdf/crane_plus.xacro old mode 100755 new mode 100644 diff --git a/crane_plus_examples/CHANGELOG.rst b/crane_plus_examples/CHANGELOG.rst old mode 100755 new mode 100644 diff --git a/crane_plus_examples/CMakeLists.txt b/crane_plus_examples/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/crane_plus_examples/README.md b/crane_plus_examples/README.md old mode 100755 new mode 100644 diff --git a/crane_plus_examples/aruco_markers.pdf b/crane_plus_examples/aruco_markers.pdf old mode 100755 new mode 100644 diff --git a/crane_plus_examples/config/camera_info.yaml b/crane_plus_examples/config/camera_info.yaml old mode 100755 new mode 100644 diff --git a/crane_plus_examples/launch/camera_example.launch.py b/crane_plus_examples/launch/camera_example.launch.py old mode 100755 new mode 100644 diff --git a/crane_plus_examples/launch/camera_example.rviz b/crane_plus_examples/launch/camera_example.rviz old mode 100755 new mode 100644 diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py old mode 100755 new mode 100644 diff --git a/crane_plus_examples/launch/example.launch.py b/crane_plus_examples/launch/example.launch.py old mode 100755 new mode 100644 diff --git a/crane_plus_examples/package.xml b/crane_plus_examples/package.xml old mode 100755 new mode 100644 diff --git a/crane_plus_examples/scripts/aruco_detection.py b/crane_plus_examples/scripts/aruco_detection.py new file mode 100644 index 00000000..e549d42b --- /dev/null +++ b/crane_plus_examples/scripts/aruco_detection.py @@ -0,0 +1,51 @@ +import rclpy +from rclpy.node import Node + +import math + +# from std_msgs.msg import String +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +import cv2 +from cv_bridge import CvBridge +from geometry_msgs.msg import Quaternion +import tf2_ros + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__("aruco_detection") + self.image_subscription = self.create_subscription( + Image, "image_raw", self.listener_callback, 10 + ) + self.image_subscription # prevent unused variable warning + + self.camera_info_subscription = self.create_subscription( + Image, "camera_info", self.listener_callback, 10 + ) + self.camera_info_subscription # prevent unused variable warning + + self.camera_info = None + + def image_callback(self, msg): + self.get_logger().info('I heard: "%s"' % msg.data) + + def camera_info_callback(self, msg): + self.get_logger().info('I heard: "%s"' % msg.data) + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples/scripts/color_detection.py b/crane_plus_examples/scripts/color_detection.py new file mode 100644 index 00000000..4ca70279 --- /dev/null +++ b/crane_plus_examples/scripts/color_detection.py @@ -0,0 +1,14 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from std_msgs.msg import String + + + +def main(args=None): + try: + except: + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/gripper_control.py b/crane_plus_examples/scripts/gripper_control.py new file mode 100644 index 00000000..37ec8e75 --- /dev/null +++ b/crane_plus_examples/scripts/gripper_control.py @@ -0,0 +1,48 @@ +import math + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_msgs.msg import String + + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + MultiPipelinePlanRequestParameters, +) + +from config import moveit_config + +def to_radians(deg_angle): + return deg_angle * math.pi / 180.0 + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("gripper_control") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py", config_dict=moveit_config) + crane_plus_gripper = crane_plus.get_planning_component("gripper") + logger.info("MoveItPy instance created") + + + + + + + + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpy.shutdown() +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/joint_values.py b/crane_plus_examples/scripts/joint_values.py new file mode 100644 index 00000000..4ca70279 --- /dev/null +++ b/crane_plus_examples/scripts/joint_values.py @@ -0,0 +1,14 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from std_msgs.msg import String + + + +def main(args=None): + try: + except: + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/pick_and_place.py b/crane_plus_examples/scripts/pick_and_place.py new file mode 100644 index 00000000..4ca70279 --- /dev/null +++ b/crane_plus_examples/scripts/pick_and_place.py @@ -0,0 +1,14 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from std_msgs.msg import String + + + +def main(args=None): + try: + except: + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/pick_and_place_tf.py b/crane_plus_examples/scripts/pick_and_place_tf.py new file mode 100644 index 00000000..4ca70279 --- /dev/null +++ b/crane_plus_examples/scripts/pick_and_place_tf.py @@ -0,0 +1,14 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from std_msgs.msg import String + + + +def main(args=None): + try: + except: + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/pose_groupstate.py b/crane_plus_examples/scripts/pose_groupstate.py new file mode 100644 index 00000000..4ca70279 --- /dev/null +++ b/crane_plus_examples/scripts/pose_groupstate.py @@ -0,0 +1,14 @@ +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from std_msgs.msg import String + + + +def main(args=None): + try: + except: + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_examples/src/pose_groupstate.cpp b/crane_plus_examples/src/pose_groupstate.cpp old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/CHANGELOG.rst b/crane_plus_gazebo/CHANGELOG.rst old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/CMakeLists.txt b/crane_plus_gazebo/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/README.md b/crane_plus_gazebo/README.md old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/gui/gui.config b/crane_plus_gazebo/gui/gui.config old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/launch/crane_plus_with_table.launch.py b/crane_plus_gazebo/launch/crane_plus_with_table.launch.py old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/package.xml b/crane_plus_gazebo/package.xml old mode 100755 new mode 100644 diff --git a/crane_plus_gazebo/worlds/table.sdf b/crane_plus_gazebo/worlds/table.sdf old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/.setup_assistant b/crane_plus_moveit_config/.setup_assistant old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/CHANGELOG.rst b/crane_plus_moveit_config/CHANGELOG.rst old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/CMakeLists.txt b/crane_plus_moveit_config/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/README.md b/crane_plus_moveit_config/README.md old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/config/controllers.yaml b/crane_plus_moveit_config/config/controllers.yaml old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/config/crane_plus.srdf b/crane_plus_moveit_config/config/crane_plus.srdf old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/config/joint_limits.yaml b/crane_plus_moveit_config/config/joint_limits.yaml old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/config/kinematics.yaml b/crane_plus_moveit_config/config/kinematics.yaml old mode 100755 new mode 100644 index 084a052c..d298df20 --- a/crane_plus_moveit_config/config/kinematics.yaml +++ b/crane_plus_moveit_config/config/kinematics.yaml @@ -1,5 +1,10 @@ +# arm: +# kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin +# kinematics_solver_attempts: 10 +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 arm: - kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin - kinematics_solver_attempts: 10 + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/crane_plus_moveit_config/config/moveit.rviz b/crane_plus_moveit_config/config/moveit.rviz old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/config/ompl_planning.yaml b/crane_plus_moveit_config/config/ompl_planning.yaml old mode 100755 new mode 100644 diff --git a/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml b/crane_plus_moveit_config/config/pilz_cartesian_limits.yaml old mode 100755 new mode 100644 From b6e6962d5e0476d0148e53957c776f1aada1e216 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 15 Aug 2024 17:52:22 +0900 Subject: [PATCH 016/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E6=A8=A9?= =?UTF-8?q?=E9=99=90=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_control/CHANGELOG.rst | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 crane_plus_control/CHANGELOG.rst diff --git a/crane_plus_control/CHANGELOG.rst b/crane_plus_control/CHANGELOG.rst old mode 100755 new mode 100644 From bde95e1cb07a100f50a5f6122fee96302569018d Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 16 Aug 2024 09:37:54 +0900 Subject: [PATCH 017/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AAscript?= =?UTF-8?q?=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/aruco_detection.py | 51 ------------------- .../scripts/color_detection.py | 14 ----- .../scripts/gripper_control.py | 48 ----------------- crane_plus_examples/scripts/joint_values.py | 14 ----- crane_plus_examples/scripts/pick_and_place.py | 14 ----- .../scripts/pick_and_place_tf.py | 14 ----- .../scripts/pose_groupstate.py | 14 ----- 7 files changed, 169 deletions(-) delete mode 100644 crane_plus_examples/scripts/aruco_detection.py delete mode 100644 crane_plus_examples/scripts/color_detection.py delete mode 100644 crane_plus_examples/scripts/gripper_control.py delete mode 100644 crane_plus_examples/scripts/joint_values.py delete mode 100644 crane_plus_examples/scripts/pick_and_place.py delete mode 100644 crane_plus_examples/scripts/pick_and_place_tf.py delete mode 100644 crane_plus_examples/scripts/pose_groupstate.py diff --git a/crane_plus_examples/scripts/aruco_detection.py b/crane_plus_examples/scripts/aruco_detection.py deleted file mode 100644 index e549d42b..00000000 --- a/crane_plus_examples/scripts/aruco_detection.py +++ /dev/null @@ -1,51 +0,0 @@ -import rclpy -from rclpy.node import Node - -import math - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo -from sensor_msgs.msg import Image -import cv2 -from cv_bridge import CvBridge -from geometry_msgs.msg import Quaternion -import tf2_ros - - -class ImageSubscriber(Node): - def __init__(self): - super().__init__("aruco_detection") - self.image_subscription = self.create_subscription( - Image, "image_raw", self.listener_callback, 10 - ) - self.image_subscription # prevent unused variable warning - - self.camera_info_subscription = self.create_subscription( - Image, "camera_info", self.listener_callback, 10 - ) - self.camera_info_subscription # prevent unused variable warning - - self.camera_info = None - - def image_callback(self, msg): - self.get_logger().info('I heard: "%s"' % msg.data) - - def camera_info_callback(self, msg): - self.get_logger().info('I heard: "%s"' % msg.data) - -def main(args=None): - rclpy.init(args=args) - - image_subscriber = ImageSubscriber() - rclpy.spin(image_subscriber) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - image_subscriber.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crane_plus_examples/scripts/color_detection.py b/crane_plus_examples/scripts/color_detection.py deleted file mode 100644 index 4ca70279..00000000 --- a/crane_plus_examples/scripts/color_detection.py +++ /dev/null @@ -1,14 +0,0 @@ -import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.node import Node - -from std_msgs.msg import String - - - -def main(args=None): - try: - except: - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/gripper_control.py b/crane_plus_examples/scripts/gripper_control.py deleted file mode 100644 index 37ec8e75..00000000 --- a/crane_plus_examples/scripts/gripper_control.py +++ /dev/null @@ -1,48 +0,0 @@ -import math - -import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.node import Node -from std_msgs.msg import String - - -# generic ros libraries -from rclpy.logging import get_logger - -# moveit python library -from moveit.core.robot_state import RobotState -from moveit.planning import ( - MoveItPy, - MultiPipelinePlanRequestParameters, -) - -from config import moveit_config - -def to_radians(deg_angle): - return deg_angle * math.pi / 180.0 - - -def main(args=None): - # ros2の初期化 - rclpy.init(args=args) - - # ロガー生成 - logger = get_logger("gripper_control") - - # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py", config_dict=moveit_config) - crane_plus_gripper = crane_plus.get_planning_component("gripper") - logger.info("MoveItPy instance created") - - - - - - - - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/joint_values.py b/crane_plus_examples/scripts/joint_values.py deleted file mode 100644 index 4ca70279..00000000 --- a/crane_plus_examples/scripts/joint_values.py +++ /dev/null @@ -1,14 +0,0 @@ -import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.node import Node - -from std_msgs.msg import String - - - -def main(args=None): - try: - except: - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/pick_and_place.py b/crane_plus_examples/scripts/pick_and_place.py deleted file mode 100644 index 4ca70279..00000000 --- a/crane_plus_examples/scripts/pick_and_place.py +++ /dev/null @@ -1,14 +0,0 @@ -import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.node import Node - -from std_msgs.msg import String - - - -def main(args=None): - try: - except: - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/pick_and_place_tf.py b/crane_plus_examples/scripts/pick_and_place_tf.py deleted file mode 100644 index 4ca70279..00000000 --- a/crane_plus_examples/scripts/pick_and_place_tf.py +++ /dev/null @@ -1,14 +0,0 @@ -import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.node import Node - -from std_msgs.msg import String - - - -def main(args=None): - try: - except: - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples/scripts/pose_groupstate.py b/crane_plus_examples/scripts/pose_groupstate.py deleted file mode 100644 index 4ca70279..00000000 --- a/crane_plus_examples/scripts/pose_groupstate.py +++ /dev/null @@ -1,14 +0,0 @@ -import rclpy -from rclpy.executors import ExternalShutdownException -from rclpy.node import Node - -from std_msgs.msg import String - - - -def main(args=None): - try: - except: - -if __name__ == '__main__': - main() \ No newline at end of file From 9013d35b9491ecaddfa4747d6dd7dda783d676b5 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 16 Aug 2024 09:46:09 +0900 Subject: [PATCH 018/111] =?UTF-8?q?kinematic=E3=81=AE=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/config/kinematics.yaml | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/crane_plus_moveit_config/config/kinematics.yaml b/crane_plus_moveit_config/config/kinematics.yaml index d298df20..084a052c 100644 --- a/crane_plus_moveit_config/config/kinematics.yaml +++ b/crane_plus_moveit_config/config/kinematics.yaml @@ -1,10 +1,5 @@ -# arm: -# kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin -# kinematics_solver_attempts: 10 -# kinematics_solver_search_resolution: 0.005 -# kinematics_solver_timeout: 0.005 arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_attempts: 10 kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 + kinematics_solver_timeout: 0.005 From 82a96db68b55eb0d2d1387f664172905b25914e4 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Tue, 20 Aug 2024 14:06:18 +0900 Subject: [PATCH 019/111] =?UTF-8?q?=E3=82=BD=E3=83=AB=E3=83=90=E3=83=BC?= =?UTF-8?q?=E3=82=92KDL=E3=81=AB=E6=88=BB=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/config/kinematics.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crane_plus_moveit_config/config/kinematics.yaml b/crane_plus_moveit_config/config/kinematics.yaml index 084a052c..a1f17d08 100644 --- a/crane_plus_moveit_config/config/kinematics.yaml +++ b/crane_plus_moveit_config/config/kinematics.yaml @@ -1,5 +1,5 @@ arm: - kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin - kinematics_solver_attempts: 10 + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file From 3199a4d8fe93898e1a277f1ede77c2786827eca0 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Mon, 9 Sep 2024 15:37:34 +0900 Subject: [PATCH 020/111] =?UTF-8?q?=E5=AE=9F=E8=A3=85=E5=AE=8C=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/crane_plus_moveit_py_examples.yaml | 26 ++ .../launch/example.launch.py | 109 +++++++ .../scripts/aruco_detection.py | 116 ++++++++ .../scripts/color_detection.py | 139 +++++++++ .../scripts/gripper_control.py | 92 ++++++ .../scripts/joint_values.py | 96 ++++++ .../scripts/pick_and_place.py | 275 ++++++++++++++++++ .../scripts/pick_and_place_tf.py | 252 ++++++++++++++++ .../scripts/pose_groupstate.py | 84 ++++++ crane_plus_examples_py/scripts/utils.py | 78 +++++ 10 files changed, 1267 insertions(+) create mode 100644 crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml create mode 100644 crane_plus_examples_py/launch/example.launch.py create mode 100644 crane_plus_examples_py/scripts/aruco_detection.py create mode 100644 crane_plus_examples_py/scripts/color_detection.py create mode 100644 crane_plus_examples_py/scripts/gripper_control.py create mode 100644 crane_plus_examples_py/scripts/joint_values.py create mode 100644 crane_plus_examples_py/scripts/pick_and_place.py create mode 100644 crane_plus_examples_py/scripts/pick_and_place_tf.py create mode 100644 crane_plus_examples_py/scripts/pose_groupstate.py create mode 100644 crane_plus_examples_py/scripts/utils.py diff --git a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml new file mode 100644 index 00000000..c439aef7 --- /dev/null +++ b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml @@ -0,0 +1,26 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: ["ompl"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +ompl_rrtc: # Namespace for individual plan request + plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp + planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem + planning_pipeline: ompl # Name of the pipeline that is being used + planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline + max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py new file mode 100644 index 00000000..e46fa01c --- /dev/null +++ b/crane_plus_examples_py/launch/example.launch.py @@ -0,0 +1,109 @@ +# Copyright 2023 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 moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + ld = LaunchDescription() + declare_loaded_description = DeclareLaunchArgument( + "loaded_description", + default_value="", + description="Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + ) + + moveit_config = ( + MoveItConfigsBuilder("crane_plus") + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .robot_description( + file_path=os.path.join( + get_package_share_directory("crane_plus_description"), + "urdf", + "crane_plus.urdf.xacro", + ), + mappings={}, + ) + .robot_description_semantic( + file_path="config/crane_plus.srdf", + mappings={"model": "crane_plus"}, + ) + .joint_limits(file_path="config/joint_limits.yaml") + .trajectory_execution( + file_path="config/controllers.yaml", moveit_manage_controllers=True + ) + .planning_pipelines(pipelines=["ompl"]) + .robot_description_kinematics(file_path="config/kinematics.yaml") + .moveit_cpp( + file_path=get_package_share_directory("crane_plus_examples_py") + + "/config/crane_plus_moveit_py_examples.yaml" + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + "robot_description": LaunchConfiguration("loaded_description") + } + + moveit_config.move_group_capabilities = {"capabilities": ""} + + declare_example_name = DeclareLaunchArgument( + "example", + default_value="gripper_control", + description=( + "Set an example executable name: " + "[gripper_control, pose_groupstate, joint_values, pick_and_place]" + ), + ) + + declare_use_sim_time = DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description=("Set true when using the gazebo simulator."), + ) + + use_sim_time_name = SetParameter( + name="use_sim_time", value=LaunchConfiguration("use_sim_time") + ) + + example_node = Node( + name=[LaunchConfiguration("example"), "_node"], + package="crane_plus_examples", + executable=LaunchConfiguration("example"), + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + ) + + ld.add_action(declare_loaded_description) + ld.add_action(declare_example_name) + ld.add_action(declare_use_sim_time) + ld.add_action(use_sim_time_name) + ld.add_action(example_node) + + return ld diff --git a/crane_plus_examples_py/scripts/aruco_detection.py b/crane_plus_examples_py/scripts/aruco_detection.py new file mode 100644 index 00000000..64734a5e --- /dev/null +++ b/crane_plus_examples_py/scripts/aruco_detection.py @@ -0,0 +1,116 @@ +# Copyright 2020 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 rclpy +from rclpy.node import Node + +import numpy as np + +# from std_msgs.msg import String +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import CameraInfo, Image +import cv2 +from cv2 import aruco +from cv_bridge import CvBridge +import tf2_ros + +from utils import rotation_matrix_to_quaternion + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__("aruco_detection") + self.image_subscription = self.create_subscription( + Image, "image_raw", self.listener_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, "camera_info", self.listener_callback, 10 + ) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + # 画像データをROSのメッセージからOpenCVの配列に変換 + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) + + if self.camera_info: + # ArUcoマーカのデータセットを読み込む + # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) + + # マーカーID + ids = [] + + # 画像座標系上のマーカ頂点位置 + corners = [] + + # マーカの検出 + corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + + # マーカの検出数 + n_markers = len(ids) + + # カメラパラメータ + CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) + DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) + + # マーカ一辺の長さ 0.04 [m] + MARKER_LENGTH = 0.04 + + # マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 + if n_markers > 0: + for i in range(n_markers): + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) + # tfの配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = "target_" + str(ids[i][0]) + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトル→回転行列 + rotation_matrix, _ = cv2.Rodrigues(rvecs[i]) + # 回転行列からクォータニオンを求める + q = rotation_matrix_to_quaternion(rotation_matrix) + t.transform.rotation.x = q.x + t.transform.rotation.y = q.y + t.transform.rotation.z = q.z + t.transform.rotation.w = q.w + self.tf_broadcaster.sendTransform(t) + + def camera_info_callback(self, msg): + self.camera_info = msg + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/scripts/color_detection.py b/crane_plus_examples_py/scripts/color_detection.py new file mode 100644 index 00000000..17ea59cf --- /dev/null +++ b/crane_plus_examples_py/scripts/color_detection.py @@ -0,0 +1,139 @@ +# Copyright 2020 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 rclpy +from rclpy.node import Node + +# from std_msgs.msg import String +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import CameraInfo, Image +from image_geometry import PinholeCameraModel +import cv2 +from cv_bridge import CvBridge +import tf2_ros + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__("color_detection") + self.image_subscription = self.create_subscription( + Image, "image_raw", self.listener_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, "camera_info", self.listener_callback, 10 + ) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + if self.camera_info: + # 赤い物体を検出するようにHSVの範囲を設定 + # 周囲の明るさ等の動作環境に合わせて調整 + LOW_H_1 = 0 + HIGH_H_1 = 20 + LOW_H_2 = 160 + HIGH_H_2 = 179 + LOW_S = 100 + HIGH_S = 255 + LOW_V = 50 + HIGH_V = 255 + + # ウェブカメラの画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + + # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) + + # 画像の二値化 + img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) + img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) + + # マスク画像の合成 + img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) + + # ノイズ除去の処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + + # 穴埋めの処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + + # 画像の検出領域におけるモーメントを計算 + moment = cv2.moments(img_thresholded) + d_m01 = moment['m01'] + d_m10 = moment['m10'] + d_area = moment['m00'] + + # 検出した領域のピクセル数が10000より大きい場合 + if d_area < 10000: + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(self.camera_info) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyImage(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # カメラの高さを0.44[m]として把持対象物の位置を計算 + CAMERA_HEIGHT = 0.46 + object_position = { + 'x': ray.x * CAMERA_HEIGHT, + 'y': ray.y * CAMERA_HEIGHT, + 'z': ray.z * CAMERA_HEIGHT, + } + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = "target_0" + t.transform.translation.x = object_position['x'] + t.transform.translation.y = object_position['y'] + t.transform.translation.z = object_position['z'] + self.tf_broadcaster.sendTransform(t) + + # 閾値による二値化画像を配信 + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8") + self.image_thresholded_publisher.publish(img_thresholded_msg) + + def camera_info_callback(self, msg): + self.camera_info = msg + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples_py/scripts/gripper_control.py b/crane_plus_examples_py/scripts/gripper_control.py new file mode 100644 index 00000000..3e771a3a --- /dev/null +++ b/crane_plus_examples_py/scripts/gripper_control.py @@ -0,0 +1,92 @@ +# Copyright 2020 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 math +import rclpy + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from utils import plan_and_execute + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("gripper_control") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_gripper = crane_plus.get_planning_component("gripper") + logger.info("MoveItPy instance created") + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) + + # planningのパラメータ設定 + plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 + + # グリッパーを+30[deg]の位置に動かす + robot_state.set_joint_group_positions("gripper", [math.radians(30.0)]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=plan_request_params, + ) + # グリッパーを-30[deg]の位置に動かす + robot_state.set_joint_group_positions("gripper", [math.radians(-30.0)]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=plan_request_params, + ) + # グリッパーを0[deg]の位置に動かす + robot_state.set_joint_group_positions("gripper", [0.0]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=plan_request_params, + ) + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/scripts/joint_values.py b/crane_plus_examples_py/scripts/joint_values.py new file mode 100644 index 00000000..a8fb9d8f --- /dev/null +++ b/crane_plus_examples_py/scripts/joint_values.py @@ -0,0 +1,96 @@ +# Copyright 2020 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 math + +import rclpy + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +from utils import plan_and_execute + + +def to_radians(deg_angle): + return deg_angle * math.pi / 180.0 + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("joint_values") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_arm = crane_plus.get_planning_component("arm") + logger.info("MoveItPy instance created") + + # 速度＆加速度のスケーリングファクタを設定 + plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 + + # 現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置からarmの関節を全て順番に45度動かす + joint_values = crane_plus_arm.get_named_target_state_values("arm") + target_joint_value = math.radians(45.0) + + # joint_valuesはDict型なので、keyの数（=jointの数）だけ関節を動かす + for key in enumerate(joint_values.keys()): + joint_values[key] = target_joint_value + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # MoveItPyの終了 + crane_plus.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/scripts/pick_and_place.py b/crane_plus_examples_py/scripts/pick_and_place.py new file mode 100644 index 00000000..e549f7d6 --- /dev/null +++ b/crane_plus_examples_py/scripts/pick_and_place.py @@ -0,0 +1,275 @@ +# Copyright 2020 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 math + +import rclpy +from geometry_msgs.msg import PoseStamped + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from utils import plan_and_execute, euler_to_quaternion + + +def to_radians(deg_angle): + return deg_angle * math.pi / 180.0 + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("pick_and_place") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_arm = crane_plus.get_planning_component("arm") + crane_plus_gripper = crane_plus.get_planning_component("gripper") + logger.info("MoveItPy instance created") + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) + + # planningのパラメータ設定 + # armのパラメータ設定用 + arm_plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + arm_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + + # gripperのパラメータ設定用 + gripper_plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + gripper_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + + # gripperの開閉角度 + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # armを現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperを0度の位置に動かす + robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # ----- Picking Preparation ----- + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # なにか掴んでいたときのためにgripperを開く + robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # armが掴みに行く位置を指定して動かす + pose_goal = PoseStamped() + pose_goal.header.frame_id = "base_link" + pose_goal.pose.position.x = 0.0 + pose_goal.pose.position.y = -0.09 + pose_goal.pose.position.z = 0.17 + q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(-90)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armの姿勢を変える(Y軸反転) + q = euler_to_quaternion(to_radians(0), to_radians(180), to_radians(-90)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armをz軸上に動かす + pose_goal.pose.position.z = 0.14 + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # Grasp + robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # armをz軸上に動かす + pose_goal.pose.position.z = 0.17 + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # ----- Placing Preparation ----- + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armが掴みに行く位置を指定して動かす + pose_goal.pose.position.x = 0.15 + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.06 + q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(0)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # Release + robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # Return to home and vertical pose + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armを現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperの開閉をデフォルトの間隔に戻す + robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/scripts/pick_and_place_tf.py b/crane_plus_examples_py/scripts/pick_and_place_tf.py new file mode 100644 index 00000000..36d51d73 --- /dev/null +++ b/crane_plus_examples_py/scripts/pick_and_place_tf.py @@ -0,0 +1,252 @@ +# Copyright 2020 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 math +import numpy as np +import datetime + +# import angles +from geometry_msgs.msg import Pose +from moveit_msgs.msg import Constraints, JointConstraint +import rclpy +from rclpy.time import Time +from rclpy.node import Node +import tf2_ros +from tf2_ros import TransformStamped, TransformListener +from tf2_ros.buffer import Buffer + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +from utils import plan_and_execute, euler_to_quaternion + + +class PickAndPlaceTf(Node): + def __init__(self, crane_plus): + super().__init__("pick_and_place_tf_node") + self.tf_past = TransformStamped() + self.crane_plus = crane_plus + self.crane_plus_arm = crane_plus.get_planning_component("arm") + self.crane_plus_gripper = crane_plus.get_planning_component("gripper") + + # instantiate a RobotState instance using the current robot model + self.robot_model = crane_plus.get_robot_model() + self.robot_state = RobotState(self.robot_model) + + # planningのパラメータ設定 + # armのパラメータ設定用 + self.arm_plan_request_params = PlanRequestParameters( + self.crane_plus, + "ompl_rrtc", + ) + self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # gripperのパラメータ設定用 + self.gripper_plan_request_params = PlanRequestParameters( + self.crane_plus, + "ompl_rrtc", + ) + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # SRDFに定義されている"home"の姿勢にする + self.crane_plus_arm.set_start_state_to_current_state() + self.crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + self.crane_plus, + self.crane_plus_arm, + logger=None, + single_plan_parameters=self.arm_plan_request_params, + ) + + # 可動範囲を制限する + constraints = Constraints() + constraints.name = "arm_constraints" + + jointConstraint = JointConstraint() + jointConstraint.joint_name = "crane_plus_joint1" + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(100) + jointConstraint.tolerance_below = math.radians(100) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + jointConstraint.joint_name = "crane_plus_joint3" + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(0) + jointConstraint.tolerance_below = math.radians(180) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + self.crane_plus_arm.set_path_constraints(constraints) + + # 待機姿勢 + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + + # tf + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # Call on_timer function every second + self.timer = self.create_timer(0.5, self.on_timer) + + def on_timer(self): + # target_0のtf位置姿勢を取得 + try: + tf_msg = self.tf_buffer.lookup_transform( + 'base_link', 'target_0', Time()) + except tf2_ros.LookupException as ex: + self.get_logger().info(f'Could not transform base_link to target: {ex}') + + now = Time() + FILTERING_TIME = datetime.timedelta(seconds=2) + STOP_TIME_THRESHOLD = datetime.timedelta(seconds=3) + DISTANCE_THRESHOLD = 0.01 + + # 経過時間と停止時間を計算(nsec) + # 経過時間 + TF_ELAPSED_TIME = now.nanoseconds - tf_msg.header.stamp.nanosec + # 停止時間 + if self.tf_past is not None: + TF_STOP_TIME = now.nanoseconds - self.tf_past.header.stamp.nanosec + else: + TF_STOP_TIME = now.nanoseconds + + # 現在時刻から2秒以内に受け取ったtfを使用 + if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: + tf_diff = np.sqrt((self.tf_past.transform.translation.x + - tf_msg.transform.translation.x) ** 2 + + (self.tf_past.transform.translation.y + - tf_msg.transform.translation.y) ** 2 + + (self.tf_past.transform.translation.z + - tf_msg.transform.translation.z) ** 2 + ) + # 把持対象の位置が停止していることを判定 + if tf_diff < DISTANCE_THRESHOLD: + # 把持対象が3秒以上停止している場合ピッキング動作開始 + if TF_STOP_TIME > STOP_TIME_THRESHOLD.total_seconds() * 1e9: + self._picking(tf_msg) + else: + self.tf_past = tf_msg + + def _picking(self, tf_msg): + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # 何かを掴んでいた時のためにハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 + x = tf_msg.transform.translation.x + y = tf_msg.transform.translation.y + theta_rad = math.atan2(y, x) + theta_deg = math.degrees(theta_rad) + + # 把持対象物に正対する + self._control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg) + + # 掴みに行く + GRIPPER_OFFSET = 0.13 + gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) + gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) + if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, + 0.05, 0, 90, theta_deg): + # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + return + + # ハンドを閉じる + self._control_gripper(GRIPPER_CLOSE) + + # 移動する + self._control_arm(0.0, 0.0, 0.17, 0, 90, 0) + + # 下ろす + self._control_arm(0.0, -0.15, 0.06, 0, 90, -90) + + # ハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # 少しだけハンドを持ち上げる + self._control_arm(0.0, -0.15, 0.10, 0, 90, -90) + + # 待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + + # ハンドを閉じる + self._control_gripper(GRIPPER_DEFAULT) + + # グリッパ制御 + def _control_gripper(self, angle): + self.robot_state.set_joint_group_positions("gripper", [angle]) + self.crane_plus_gripper.set_start_state_to_current_state() + self.crane_plus_gripper.set_goal_state(robot_state=self.robot_state) + plan_and_execute( + self.crane_plus, + self.crane_plus_gripper, + logger=None, + single_plan_parameters=self.gripper_plan_request_params, + ) + + # アーム制御 + def _control_arm(self, x, y, z, roll, pitch, yaw): + target_pose = Pose() + target_pose.position.x = x + target_pose.position.y = y + target_pose.position.z = z + q = euler_to_quaternion(math.radians(roll), math.radians(pitch), + math.radians(yaw)) + target_pose.orientation.x = q[0] + target_pose.orientation.y = q[1] + target_pose.orientation.z = q[2] + target_pose.orientation.w = q[3] + self.crane_plus_arm.set_start_state_to_current_state() + self.crane_plus_arm.set_goal_state( + pose_stamped_msg=target_pose, pose_link="crane_plus_link4" + ) + result = plan_and_execute( + self.crane_plus, + self.crane_plus_arm, + logger=None, + single_plan_parameters=self.arm_plan_request_params, + ) + return result + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + + # node生成 + pick_and_place_tf_node = PickAndPlaceTf(crane_plus) + rclpy.spin(pick_and_place_tf_node) + + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples_py/scripts/pose_groupstate.py b/crane_plus_examples_py/scripts/pose_groupstate.py new file mode 100644 index 00000000..f3fc13bd --- /dev/null +++ b/crane_plus_examples_py/scripts/pose_groupstate.py @@ -0,0 +1,84 @@ + +# Copyright 2020 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 rclpy + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from utils import plan_and_execute + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("pose_groupstate") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_arm = crane_plus.get_planning_component("arm") + logger.info("MoveItPy instance created") + + # planningのパラメータ設定 + plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # 現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # MoveItPyの終了 + crane_plus.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/scripts/utils.py b/crane_plus_examples_py/scripts/utils.py new file mode 100644 index 00000000..6ad5c653 --- /dev/null +++ b/crane_plus_examples_py/scripts/utils.py @@ -0,0 +1,78 @@ +# Copyright 2020 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 time +import numpy as np +import quaternion + + +# plan and execute +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + # plan to goal + logger.info("Planning trajectory") + if multi_plan_parameters is not None: + plan_result = planning_component.plan( + multi_plan_parameters=multi_plan_parameters + ) + elif single_plan_parameters is not None: + plan_result = planning_component.plan( + single_plan_parameters=single_plan_parameters + ) + else: + plan_result = planning_component.plan() + + # execute the plan + result = None + if plan_result: + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + result = robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + result = False + time.sleep(sleep_time) + return result + + +# euler --> quaternion +def euler_to_quaternion(roll, pitch, yaw): + cy = np.cos(yaw * 0.5) + sy = np.sin(yaw * 0.5) + cr = np.cos(roll * 0.5) + sr = np.sin(roll * 0.5) + cp = np.cos(pitch * 0.5) + sp = np.sin(pitch * 0.5) + + qw = cy * cr * cp + sy * sr * sp + qx = cy * sr * cp - sy * cr * sp + qy = cy * cr * sp + sy * sr * cp + qz = sy * cr * cp - cy * sr * sp + + return [qx, qy, qz, qw] + + +# rotation matrix --> quaternion +def rotation_matrix_to_quaternion(rotation_matrix): + # numpy-quaternionを使用して回転行列からクォータニオンを計算 + #3x3の回転行列をnumpy.quaternionに変換する + q = quaternion.from_rotation_matrix(rotation_matrix) + return q \ No newline at end of file From d31d5e62ee8b918f9e44aca2140123937a48f505 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Wed, 18 Sep 2024 15:48:30 +0900 Subject: [PATCH 021/111] =?UTF-8?q?jazzy=E7=92=B0=E5=A2=83=E5=AF=BE?= =?UTF-8?q?=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_description/package.xml | 3 +-- crane_plus_examples/src/aruco_detection.cpp | 2 +- crane_plus_examples/src/color_detection.cpp | 4 ++-- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/crane_plus_description/package.xml b/crane_plus_description/package.xml index cf986cda..1e5ff7ec 100644 --- a/crane_plus_description/package.xml +++ b/crane_plus_description/package.xml @@ -13,8 +13,7 @@ ament_cmake ament_cmake_python - gazebo_ros2_control - ign_ros2_control + gz_ros2_control joint_state_publisher_gui launch robot_state_publisher diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index bdf9b19d..7a70f071 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -27,7 +27,7 @@ #include "sensor_msgs/msg/image.hpp" #include "opencv2/opencv.hpp" #include "opencv2/aruco.hpp" -#include "cv_bridge/cv_bridge.h" +#include "cv_bridge/cv_bridge.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_ros/transform_broadcaster.h" diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp index 8590e51e..73d98682 100644 --- a/crane_plus_examples/src/color_detection.cpp +++ b/crane_plus_examples/src/color_detection.cpp @@ -29,8 +29,8 @@ #include "tf2_ros/transform_broadcaster.h" #include "opencv2/opencv.hpp" #include "opencv2/imgproc/imgproc.hpp" -#include "cv_bridge/cv_bridge.h" -#include "image_geometry/pinhole_camera_model.h" +#include "cv_bridge/cv_bridge.hpp" +#include "image_geometry/pinhole_camera_model.hpp" using std::placeholders::_1; class ImageSubscriber : public rclcpp::Node From a057fce489540e431dd661b859ddb2f36230c7db Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 19 Sep 2024 11:28:50 +0900 Subject: [PATCH 022/111] =?UTF-8?q?camera=5Fexample.launch.py=E3=81=AE?= =?UTF-8?q?=E5=AE=9F=E8=A3=85=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/camera_example.launch.py | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 crane_plus_examples_py/launch/camera_example.launch.py diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py new file mode 100644 index 00000000..ac966fcd --- /dev/null +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -0,0 +1,93 @@ +# Copyright 2023 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 moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = ( + MoveItConfigsBuilder("crane_plus") + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .robot_description( + file_path=os.path.join( + get_package_share_directory("crane_plus_description"), + "urdf", + "crane_plus.urdf.xacro", + ), + mappings={}, + ) + .robot_description_semantic( + file_path="config/crane_plus.srdf", + mappings={"model": "crane_plus"}, + ) + .joint_limits(file_path="config/joint_limits.yaml") + .trajectory_execution( + file_path="config/controllers.yaml", moveit_manage_controllers=True + ) + .planning_pipelines(pipelines=["ompl"]) + .robot_description_kinematics(file_path="config/kinematics.yaml") + .moveit_cpp( + file_path=get_package_share_directory("crane_plus_examples_py") + + "/config/crane_plus_moveit_py_examples.yaml" + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + "robot_description": LaunchConfiguration("loaded_description") + } + + moveit_config.move_group_capabilities = {"capabilities": ""} + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='color_detection', + description=('Set an example executable name: ' + '[color_detection]') + ) + + picking_node = Node( + name='pick_and_place_tf', + package="crane_plus_examples_py", + executable='pick_and_place_tf', + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + ) + + detection_node = Node( + name=[LaunchConfiguration("example"), "_node"], + package="crane_plus_examples_py", + executable=LaunchConfiguration("example"), + output="screen" + ) + + ld = LaunchDescription() + ld.add_action(detection_node) + ld.add_action(picking_node) + ld.add_action(declare_example_name) + + return ld From e7ce02dd413892ab7a6c9a2d7189871000969697 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 19 Sep 2024 14:57:17 +0900 Subject: [PATCH 023/111] add crane_plus_example_py --- crane_plus_examples_py/LICENSE | 202 +++++++++++++ .../crane_plus_examples_py/__init__.py | 0 .../crane_plus_examples_py/aruco_detection.py | 116 ++++++++ .../crane_plus_examples_py/color_detection.py | 139 +++++++++ .../crane_plus_examples_py/gripper_control.py | 92 ++++++ .../crane_plus_examples_py/joint_values.py | 96 ++++++ .../crane_plus_examples_py/pick_and_place.py | 275 ++++++++++++++++++ .../pick_and_place_tf.py | 252 ++++++++++++++++ .../crane_plus_examples_py/pose_groupstate.py | 84 ++++++ .../crane_plus_examples_py/utils.py | 78 +++++ crane_plus_examples_py/package.xml | 18 ++ .../resource/crane_plus_examples_py | 0 crane_plus_examples_py/setup.cfg | 4 + crane_plus_examples_py/setup.py | 37 +++ crane_plus_examples_py/test/test_copyright.py | 25 ++ crane_plus_examples_py/test/test_flake8.py | 25 ++ crane_plus_examples_py/test/test_pep257.py | 23 ++ 17 files changed, 1466 insertions(+) create mode 100644 crane_plus_examples_py/LICENSE create mode 100644 crane_plus_examples_py/crane_plus_examples_py/__init__.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/color_detection.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/gripper_control.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/joint_values.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py create mode 100644 crane_plus_examples_py/crane_plus_examples_py/utils.py create mode 100644 crane_plus_examples_py/package.xml create mode 100644 crane_plus_examples_py/resource/crane_plus_examples_py create mode 100644 crane_plus_examples_py/setup.cfg create mode 100644 crane_plus_examples_py/setup.py create mode 100644 crane_plus_examples_py/test/test_copyright.py create mode 100644 crane_plus_examples_py/test/test_flake8.py create mode 100644 crane_plus_examples_py/test/test_pep257.py diff --git a/crane_plus_examples_py/LICENSE b/crane_plus_examples_py/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/crane_plus_examples_py/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/crane_plus_examples_py/crane_plus_examples_py/__init__.py b/crane_plus_examples_py/crane_plus_examples_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py new file mode 100644 index 00000000..64734a5e --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -0,0 +1,116 @@ +# Copyright 2020 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 rclpy +from rclpy.node import Node + +import numpy as np + +# from std_msgs.msg import String +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import CameraInfo, Image +import cv2 +from cv2 import aruco +from cv_bridge import CvBridge +import tf2_ros + +from utils import rotation_matrix_to_quaternion + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__("aruco_detection") + self.image_subscription = self.create_subscription( + Image, "image_raw", self.listener_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, "camera_info", self.listener_callback, 10 + ) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + # 画像データをROSのメッセージからOpenCVの配列に変換 + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) + + if self.camera_info: + # ArUcoマーカのデータセットを読み込む + # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) + + # マーカーID + ids = [] + + # 画像座標系上のマーカ頂点位置 + corners = [] + + # マーカの検出 + corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + + # マーカの検出数 + n_markers = len(ids) + + # カメラパラメータ + CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) + DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) + + # マーカ一辺の長さ 0.04 [m] + MARKER_LENGTH = 0.04 + + # マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 + if n_markers > 0: + for i in range(n_markers): + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) + # tfの配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = "target_" + str(ids[i][0]) + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトル→回転行列 + rotation_matrix, _ = cv2.Rodrigues(rvecs[i]) + # 回転行列からクォータニオンを求める + q = rotation_matrix_to_quaternion(rotation_matrix) + t.transform.rotation.x = q.x + t.transform.rotation.y = q.y + t.transform.rotation.z = q.z + t.transform.rotation.w = q.w + self.tf_broadcaster.sendTransform(t) + + def camera_info_callback(self, msg): + self.camera_info = msg + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py new file mode 100644 index 00000000..17ea59cf --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -0,0 +1,139 @@ +# Copyright 2020 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 rclpy +from rclpy.node import Node + +# from std_msgs.msg import String +from geometry_msgs.msg import TransformStamped +from sensor_msgs.msg import CameraInfo, Image +from image_geometry import PinholeCameraModel +import cv2 +from cv_bridge import CvBridge +import tf2_ros + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__("color_detection") + self.image_subscription = self.create_subscription( + Image, "image_raw", self.listener_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, "camera_info", self.listener_callback, 10 + ) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + if self.camera_info: + # 赤い物体を検出するようにHSVの範囲を設定 + # 周囲の明るさ等の動作環境に合わせて調整 + LOW_H_1 = 0 + HIGH_H_1 = 20 + LOW_H_2 = 160 + HIGH_H_2 = 179 + LOW_S = 100 + HIGH_S = 255 + LOW_V = 50 + HIGH_V = 255 + + # ウェブカメラの画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + + # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) + + # 画像の二値化 + img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) + img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) + + # マスク画像の合成 + img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) + + # ノイズ除去の処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + + # 穴埋めの処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + + # 画像の検出領域におけるモーメントを計算 + moment = cv2.moments(img_thresholded) + d_m01 = moment['m01'] + d_m10 = moment['m10'] + d_area = moment['m00'] + + # 検出した領域のピクセル数が10000より大きい場合 + if d_area < 10000: + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(self.camera_info) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyImage(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # カメラの高さを0.44[m]として把持対象物の位置を計算 + CAMERA_HEIGHT = 0.46 + object_position = { + 'x': ray.x * CAMERA_HEIGHT, + 'y': ray.y * CAMERA_HEIGHT, + 'z': ray.z * CAMERA_HEIGHT, + } + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = "target_0" + t.transform.translation.x = object_position['x'] + t.transform.translation.y = object_position['y'] + t.transform.translation.z = object_position['z'] + self.tf_broadcaster.sendTransform(t) + + # 閾値による二値化画像を配信 + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8") + self.image_thresholded_publisher.publish(img_thresholded_msg) + + def camera_info_callback(self, msg): + self.camera_info = msg + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py new file mode 100644 index 00000000..3e771a3a --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -0,0 +1,92 @@ +# Copyright 2020 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 math +import rclpy + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from utils import plan_and_execute + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("gripper_control") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_gripper = crane_plus.get_planning_component("gripper") + logger.info("MoveItPy instance created") + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) + + # planningのパラメータ設定 + plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 + + # グリッパーを+30[deg]の位置に動かす + robot_state.set_joint_group_positions("gripper", [math.radians(30.0)]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=plan_request_params, + ) + # グリッパーを-30[deg]の位置に動かす + robot_state.set_joint_group_positions("gripper", [math.radians(-30.0)]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=plan_request_params, + ) + # グリッパーを0[deg]の位置に動かす + robot_state.set_joint_group_positions("gripper", [0.0]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=plan_request_params, + ) + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py new file mode 100644 index 00000000..a8fb9d8f --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -0,0 +1,96 @@ +# Copyright 2020 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 math + +import rclpy + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +from utils import plan_and_execute + + +def to_radians(deg_angle): + return deg_angle * math.pi / 180.0 + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("joint_values") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_arm = crane_plus.get_planning_component("arm") + logger.info("MoveItPy instance created") + + # 速度＆加速度のスケーリングファクタを設定 + plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 + + # 現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置からarmの関節を全て順番に45度動かす + joint_values = crane_plus_arm.get_named_target_state_values("arm") + target_joint_value = math.radians(45.0) + + # joint_valuesはDict型なので、keyの数（=jointの数）だけ関節を動かす + for key in enumerate(joint_values.keys()): + joint_values[key] = target_joint_value + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # MoveItPyの終了 + crane_plus.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py new file mode 100644 index 00000000..e549f7d6 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -0,0 +1,275 @@ +# Copyright 2020 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 math + +import rclpy +from geometry_msgs.msg import PoseStamped + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from utils import plan_and_execute, euler_to_quaternion + + +def to_radians(deg_angle): + return deg_angle * math.pi / 180.0 + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("pick_and_place") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_arm = crane_plus.get_planning_component("arm") + crane_plus_gripper = crane_plus.get_planning_component("gripper") + logger.info("MoveItPy instance created") + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) + + # planningのパラメータ設定 + # armのパラメータ設定用 + arm_plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + arm_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + + # gripperのパラメータ設定用 + gripper_plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + gripper_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + + # gripperの開閉角度 + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # armを現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperを0度の位置に動かす + robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # ----- Picking Preparation ----- + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # なにか掴んでいたときのためにgripperを開く + robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # armが掴みに行く位置を指定して動かす + pose_goal = PoseStamped() + pose_goal.header.frame_id = "base_link" + pose_goal.pose.position.x = 0.0 + pose_goal.pose.position.y = -0.09 + pose_goal.pose.position.z = 0.17 + q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(-90)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armの姿勢を変える(Y軸反転) + q = euler_to_quaternion(to_radians(0), to_radians(180), to_radians(-90)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armをz軸上に動かす + pose_goal.pose.position.z = 0.14 + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # Grasp + robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # armをz軸上に動かす + pose_goal.pose.position.z = 0.17 + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # ----- Placing Preparation ----- + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armが掴みに行く位置を指定して動かす + pose_goal.pose.position.x = 0.15 + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.06 + q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(0)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # Release + robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # Return to home and vertical pose + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # armを現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperの開閉をデフォルトの間隔に戻す + robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py new file mode 100644 index 00000000..36d51d73 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -0,0 +1,252 @@ +# Copyright 2020 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 math +import numpy as np +import datetime + +# import angles +from geometry_msgs.msg import Pose +from moveit_msgs.msg import Constraints, JointConstraint +import rclpy +from rclpy.time import Time +from rclpy.node import Node +import tf2_ros +from tf2_ros import TransformStamped, TransformListener +from tf2_ros.buffer import Buffer + +# moveit python library +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +from utils import plan_and_execute, euler_to_quaternion + + +class PickAndPlaceTf(Node): + def __init__(self, crane_plus): + super().__init__("pick_and_place_tf_node") + self.tf_past = TransformStamped() + self.crane_plus = crane_plus + self.crane_plus_arm = crane_plus.get_planning_component("arm") + self.crane_plus_gripper = crane_plus.get_planning_component("gripper") + + # instantiate a RobotState instance using the current robot model + self.robot_model = crane_plus.get_robot_model() + self.robot_state = RobotState(self.robot_model) + + # planningのパラメータ設定 + # armのパラメータ設定用 + self.arm_plan_request_params = PlanRequestParameters( + self.crane_plus, + "ompl_rrtc", + ) + self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # gripperのパラメータ設定用 + self.gripper_plan_request_params = PlanRequestParameters( + self.crane_plus, + "ompl_rrtc", + ) + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # SRDFに定義されている"home"の姿勢にする + self.crane_plus_arm.set_start_state_to_current_state() + self.crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + self.crane_plus, + self.crane_plus_arm, + logger=None, + single_plan_parameters=self.arm_plan_request_params, + ) + + # 可動範囲を制限する + constraints = Constraints() + constraints.name = "arm_constraints" + + jointConstraint = JointConstraint() + jointConstraint.joint_name = "crane_plus_joint1" + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(100) + jointConstraint.tolerance_below = math.radians(100) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + jointConstraint.joint_name = "crane_plus_joint3" + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(0) + jointConstraint.tolerance_below = math.radians(180) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + self.crane_plus_arm.set_path_constraints(constraints) + + # 待機姿勢 + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + + # tf + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # Call on_timer function every second + self.timer = self.create_timer(0.5, self.on_timer) + + def on_timer(self): + # target_0のtf位置姿勢を取得 + try: + tf_msg = self.tf_buffer.lookup_transform( + 'base_link', 'target_0', Time()) + except tf2_ros.LookupException as ex: + self.get_logger().info(f'Could not transform base_link to target: {ex}') + + now = Time() + FILTERING_TIME = datetime.timedelta(seconds=2) + STOP_TIME_THRESHOLD = datetime.timedelta(seconds=3) + DISTANCE_THRESHOLD = 0.01 + + # 経過時間と停止時間を計算(nsec) + # 経過時間 + TF_ELAPSED_TIME = now.nanoseconds - tf_msg.header.stamp.nanosec + # 停止時間 + if self.tf_past is not None: + TF_STOP_TIME = now.nanoseconds - self.tf_past.header.stamp.nanosec + else: + TF_STOP_TIME = now.nanoseconds + + # 現在時刻から2秒以内に受け取ったtfを使用 + if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: + tf_diff = np.sqrt((self.tf_past.transform.translation.x + - tf_msg.transform.translation.x) ** 2 + + (self.tf_past.transform.translation.y + - tf_msg.transform.translation.y) ** 2 + + (self.tf_past.transform.translation.z + - tf_msg.transform.translation.z) ** 2 + ) + # 把持対象の位置が停止していることを判定 + if tf_diff < DISTANCE_THRESHOLD: + # 把持対象が3秒以上停止している場合ピッキング動作開始 + if TF_STOP_TIME > STOP_TIME_THRESHOLD.total_seconds() * 1e9: + self._picking(tf_msg) + else: + self.tf_past = tf_msg + + def _picking(self, tf_msg): + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # 何かを掴んでいた時のためにハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 + x = tf_msg.transform.translation.x + y = tf_msg.transform.translation.y + theta_rad = math.atan2(y, x) + theta_deg = math.degrees(theta_rad) + + # 把持対象物に正対する + self._control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg) + + # 掴みに行く + GRIPPER_OFFSET = 0.13 + gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) + gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) + if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, + 0.05, 0, 90, theta_deg): + # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + return + + # ハンドを閉じる + self._control_gripper(GRIPPER_CLOSE) + + # 移動する + self._control_arm(0.0, 0.0, 0.17, 0, 90, 0) + + # 下ろす + self._control_arm(0.0, -0.15, 0.06, 0, 90, -90) + + # ハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # 少しだけハンドを持ち上げる + self._control_arm(0.0, -0.15, 0.10, 0, 90, -90) + + # 待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + + # ハンドを閉じる + self._control_gripper(GRIPPER_DEFAULT) + + # グリッパ制御 + def _control_gripper(self, angle): + self.robot_state.set_joint_group_positions("gripper", [angle]) + self.crane_plus_gripper.set_start_state_to_current_state() + self.crane_plus_gripper.set_goal_state(robot_state=self.robot_state) + plan_and_execute( + self.crane_plus, + self.crane_plus_gripper, + logger=None, + single_plan_parameters=self.gripper_plan_request_params, + ) + + # アーム制御 + def _control_arm(self, x, y, z, roll, pitch, yaw): + target_pose = Pose() + target_pose.position.x = x + target_pose.position.y = y + target_pose.position.z = z + q = euler_to_quaternion(math.radians(roll), math.radians(pitch), + math.radians(yaw)) + target_pose.orientation.x = q[0] + target_pose.orientation.y = q[1] + target_pose.orientation.z = q[2] + target_pose.orientation.w = q[3] + self.crane_plus_arm.set_start_state_to_current_state() + self.crane_plus_arm.set_goal_state( + pose_stamped_msg=target_pose, pose_link="crane_plus_link4" + ) + result = plan_and_execute( + self.crane_plus, + self.crane_plus_arm, + logger=None, + single_plan_parameters=self.arm_plan_request_params, + ) + return result + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + + # node生成 + pick_and_place_tf_node = PickAndPlaceTf(crane_plus) + rclpy.spin(pick_and_place_tf_node) + + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py new file mode 100644 index 00000000..f3fc13bd --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -0,0 +1,84 @@ + +# Copyright 2020 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 rclpy + +# generic ros libraries +from rclpy.logging import get_logger + +# moveit python library +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from utils import plan_and_execute + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # ロガー生成 + logger = get_logger("pose_groupstate") + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name="moveit_py") + crane_plus_arm = crane_plus.get_planning_component("arm") + logger.info("MoveItPy instance created") + + # planningのパラメータ設定 + plan_request_params = PlanRequestParameters( + crane_plus, + "ompl_rrtc", + ) + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # 現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # 現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # MoveItPyの終了 + crane_plus.shutdown() + + +if __name__ == "__main__": + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py new file mode 100644 index 00000000..6ad5c653 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -0,0 +1,78 @@ +# Copyright 2020 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 time +import numpy as np +import quaternion + + +# plan and execute +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + """Helper function to plan and execute a motion.""" + # plan to goal + logger.info("Planning trajectory") + if multi_plan_parameters is not None: + plan_result = planning_component.plan( + multi_plan_parameters=multi_plan_parameters + ) + elif single_plan_parameters is not None: + plan_result = planning_component.plan( + single_plan_parameters=single_plan_parameters + ) + else: + plan_result = planning_component.plan() + + # execute the plan + result = None + if plan_result: + logger.info("Executing plan") + robot_trajectory = plan_result.trajectory + result = robot.execute(robot_trajectory, controllers=[]) + else: + logger.error("Planning failed") + result = False + time.sleep(sleep_time) + return result + + +# euler --> quaternion +def euler_to_quaternion(roll, pitch, yaw): + cy = np.cos(yaw * 0.5) + sy = np.sin(yaw * 0.5) + cr = np.cos(roll * 0.5) + sr = np.sin(roll * 0.5) + cp = np.cos(pitch * 0.5) + sp = np.sin(pitch * 0.5) + + qw = cy * cr * cp + sy * sr * sp + qx = cy * sr * cp - sy * cr * sp + qy = cy * cr * sp + sy * sr * cp + qz = sy * cr * cp - cy * sr * sp + + return [qx, qy, qz, qw] + + +# rotation matrix --> quaternion +def rotation_matrix_to_quaternion(rotation_matrix): + # numpy-quaternionを使用して回転行列からクォータニオンを計算 + #3x3の回転行列をnumpy.quaternionに変換する + q = quaternion.from_rotation_matrix(rotation_matrix) + return q \ No newline at end of file diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml new file mode 100644 index 00000000..a94c3713 --- /dev/null +++ b/crane_plus_examples_py/package.xml @@ -0,0 +1,18 @@ + + + + crane_plus_examples_py + 0.0.0 + TODO: Package description + root + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/crane_plus_examples_py/resource/crane_plus_examples_py b/crane_plus_examples_py/resource/crane_plus_examples_py new file mode 100644 index 00000000..e69de29b diff --git a/crane_plus_examples_py/setup.cfg b/crane_plus_examples_py/setup.cfg new file mode 100644 index 00000000..54c93bce --- /dev/null +++ b/crane_plus_examples_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/crane_plus_examples_py +[install] +install_scripts=$base/lib/crane_plus_examples_py diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py new file mode 100644 index 00000000..32fbe68b --- /dev/null +++ b/crane_plus_examples_py/setup.py @@ -0,0 +1,37 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'crane_plus_examples_py' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]'))), + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='user', + maintainer_email='user@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'aruco_detection = package_name.aruco_detectio:main', + 'color_detection = package_name.color_detection:main', + 'gripper_control = package_name.gripper_control:main', + 'joint_values = package_name.joint_values:main', + 'pick_and_place_tf = package_name.pick_and_place_tf:main', + 'pick_and_place = package_name.pick_and_place:main', + 'pose_groupstate= package_name.pose_groupstate:main', + 'utils = package_name.utils:main', + ], + }, +) diff --git a/crane_plus_examples_py/test/test_copyright.py b/crane_plus_examples_py/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/crane_plus_examples_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/crane_plus_examples_py/test/test_flake8.py b/crane_plus_examples_py/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/crane_plus_examples_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/crane_plus_examples_py/test/test_pep257.py b/crane_plus_examples_py/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/crane_plus_examples_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 91add60423440e97cc23b642db469e8f34c79b52 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 19 Sep 2024 15:21:16 +0900 Subject: [PATCH 024/111] =?UTF-8?q?warehouse=5Fros=5Fmongo=E3=82=92crane?= =?UTF-8?q?=5Fplus=5Fmoveit=5Fconfig=E3=81=AEpackage.xml=E3=81=AB=E5=90=AB?= =?UTF-8?q?=E3=82=81=E3=81=AA=E3=81=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/crane_plus_moveit_config/package.xml b/crane_plus_moveit_config/package.xml index 5e8554ba..879c2211 100755 --- a/crane_plus_moveit_config/package.xml +++ b/crane_plus_moveit_config/package.xml @@ -36,7 +36,6 @@ rviz_common rviz_default_plugins tf2_ros - warehouse_ros_mongo ament_lint_auto ament_lint_common From b8f40d7a492337d61631e3fd238db6c77cde85aa Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 20 Sep 2024 11:11:41 +0900 Subject: [PATCH 025/111] =?UTF-8?q?1=E5=9B=9E=E3=81=AE=E9=80=9A=E4=BF=A1?= =?UTF-8?q?=E3=82=A8=E3=83=A9=E3=83=BC=E3=81=A7=E7=B5=82=E4=BA=86=E5=87=A6?= =?UTF-8?q?=E7=90=86=E3=82=92=E3=81=99=E3=82=8B=E3=81=AE=E3=82=92=E3=82=84?= =?UTF-8?q?=E3=82=81=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_control/src/crane_plus_hardware.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index b1b9e7f8..7a58954e 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -205,7 +205,6 @@ return_type CranePlusHardware::read( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - return return_type::ERROR; } else { for (uint i = 0; i < hw_position_states_.size(); ++i) { hw_position_states_[i] = joint_positions[i]; @@ -271,7 +270,6 @@ return_type CranePlusHardware::write( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - return return_type::ERROR; } prev_comm_timestamp_ = steady_clock_.now(); From e33e9daeb16c5545191e2c7490c9fda00297723d Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 20 Sep 2024 14:30:20 +0900 Subject: [PATCH 026/111] =?UTF-8?q?entrypoint.sh=E3=81=AE=E3=83=91?= =?UTF-8?q?=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8=E5=90=8D=E3=81=AE=E6=8C=87?= =?UTF-8?q?=E5=AE=9A=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/setup.py | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 32fbe68b..e6a0e1bf 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -6,7 +6,8 @@ setup( name=package_name, - version='0.0.0', + # version='0.0.0', + version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', @@ -24,14 +25,14 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'aruco_detection = package_name.aruco_detectio:main', - 'color_detection = package_name.color_detection:main', - 'gripper_control = package_name.gripper_control:main', - 'joint_values = package_name.joint_values:main', - 'pick_and_place_tf = package_name.pick_and_place_tf:main', - 'pick_and_place = package_name.pick_and_place:main', - 'pose_groupstate= package_name.pose_groupstate:main', - 'utils = package_name.utils:main', + 'aruco_detection = crane_plus_examples_py.aruco_detectio:main', + 'color_detection = crane_plus_examples_py.color_detection:main', + 'gripper_control = crane_plus_examples_py.gripper_control:main', + 'joint_values = crane_plus_examples_py.joint_values:main', + 'pick_and_place_tf = crane_plus_examples_py.pick_and_place_tf:main', + 'pick_and_place = crane_plus_examples_py.pick_and_place:main', + 'pose_groupstate= crane_plus_examples_py.pose_groupstate:main', + 'utils = crane_plus_examples_py.utils:main', ], }, ) From 339805789b687619e3498d22a602b09c0d72190f Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 20 Sep 2024 14:30:57 +0900 Subject: [PATCH 027/111] =?UTF-8?q?launch=E3=83=95=E3=82=A1=E3=82=A4?= =?UTF-8?q?=E3=83=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/camera_example.launch.py | 1 + .../launch/example.launch.py | 94 ++++++++++--------- 2 files changed, 50 insertions(+), 45 deletions(-) diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index ac966fcd..3fdc3ef0 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -15,6 +15,7 @@ import os from ament_index_python.packages import get_package_share_directory +from crane_plus_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index e46fa01c..7c558ff3 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -15,6 +15,7 @@ import os from ament_index_python.packages import get_package_share_directory +from crane_plus_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -24,38 +25,44 @@ def generate_launch_description(): + # declare_loaded_description = DeclareLaunchArgument( + # "loaded_description", + # default_value="", + # description="Set robot_description text. \ + # It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + # ) ld = LaunchDescription() - declare_loaded_description = DeclareLaunchArgument( - "loaded_description", - default_value="", - description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + description_loader = RobotDescriptionLoader() + ld.add_action( + DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description="Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description." + ) ) - + moveit_config = ( MoveItConfigsBuilder("crane_plus") .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) - .robot_description( - file_path=os.path.join( - get_package_share_directory("crane_plus_description"), - "urdf", - "crane_plus.urdf.xacro", - ), - mappings={}, - ) - .robot_description_semantic( - file_path="config/crane_plus.srdf", - mappings={"model": "crane_plus"}, - ) - .joint_limits(file_path="config/joint_limits.yaml") - .trajectory_execution( - file_path="config/controllers.yaml", moveit_manage_controllers=True - ) + # .robot_description( + # file_path=os.path.join( + # get_package_share_directory("crane_plus_description"), + # "urdf", + # "crane_plus.urdf.xacro", + # ), + # mappings={}, + # ) + # .robot_description_semantic( + # file_path="config/crane_plus.srdf", + # mappings={"model": "crane_plus"}, + # ) + # .joint_limits(file_path="config/joint_limits.yaml") + .trajectory_execution(file_path="config/controllers.yaml") .planning_pipelines(pipelines=["ompl"]) - .robot_description_kinematics(file_path="config/kinematics.yaml") .moveit_cpp( file_path=get_package_share_directory("crane_plus_examples_py") + "/config/crane_plus_moveit_py_examples.yaml" @@ -70,40 +77,37 @@ def generate_launch_description(): moveit_config.move_group_capabilities = {"capabilities": ""} declare_example_name = DeclareLaunchArgument( - "example", - default_value="gripper_control", + 'example', + default_value='gripper_control', description=( - "Set an example executable name: " - "[gripper_control, pose_groupstate, joint_values, pick_and_place]" + 'Set an example executable name: ' + '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' ), ) - declare_use_sim_time = DeclareLaunchArgument( - "use_sim_time", - default_value="false", - description=("Set true when using the gazebo simulator."), - ) + # declare_use_sim_time = DeclareLaunchArgument( + # "use_sim_time", + # default_value="false", + # description=("Set true when using the gazebo simulator."), + # ) - use_sim_time_name = SetParameter( - name="use_sim_time", value=LaunchConfiguration("use_sim_time") - ) + # use_sim_time_name = SetParameter( + # name="use_sim_time", value=LaunchConfiguration("use_sim_time") + # ) example_node = Node( name=[LaunchConfiguration("example"), "_node"], - package="crane_plus_examples", + package="crane_plus_examples_py", executable=LaunchConfiguration("example"), output="screen", - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, - ], + parameters=[moveit_config.to_dict()], ) - - ld.add_action(declare_loaded_description) + + # ld = LaunchDescription([SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time'))]) + # ld.add_action(declare_use_sim_time) + # ld.add_action(use_sim_time_name) + # ld.add_action(declare_loaded_description) ld.add_action(declare_example_name) - ld.add_action(declare_use_sim_time) - ld.add_action(use_sim_time_name) ld.add_action(example_node) return ld From 9a099d31a519103940cf8e52324b0a3954481469 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 20 Sep 2024 14:31:50 +0900 Subject: [PATCH 028/111] =?UTF-8?q?package.xml=E3=81=AB=E5=BF=85=E8=A6=81?= =?UTF-8?q?=E3=81=AApython=E3=83=91=E3=83=83=E3=82=B1=E3=83=BC=E3=82=B8?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/package.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index a94c3713..de45bb77 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -7,6 +7,11 @@ root Apache-2.0 + rclpy + std_msgs + moveit + moveit_py + ament_copyright ament_flake8 ament_pep257 From 44bf384edfcf9380b8574b7b8b020d544b8d2d02 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 20 Sep 2024 16:46:26 +0900 Subject: [PATCH 029/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AAscripts?= =?UTF-8?q?=E3=83=87=E3=82=A3=E3=83=AC=E3=82=AF=E3=83=88=E3=83=AA=E5=89=8A?= =?UTF-8?q?=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../scripts/aruco_detection.py | 116 -------- .../scripts/color_detection.py | 139 --------- .../scripts/gripper_control.py | 92 ------ .../scripts/joint_values.py | 96 ------ .../scripts/pick_and_place.py | 275 ------------------ .../scripts/pick_and_place_tf.py | 252 ---------------- .../scripts/pose_groupstate.py | 84 ------ crane_plus_examples_py/scripts/utils.py | 78 ----- 8 files changed, 1132 deletions(-) delete mode 100644 crane_plus_examples_py/scripts/aruco_detection.py delete mode 100644 crane_plus_examples_py/scripts/color_detection.py delete mode 100644 crane_plus_examples_py/scripts/gripper_control.py delete mode 100644 crane_plus_examples_py/scripts/joint_values.py delete mode 100644 crane_plus_examples_py/scripts/pick_and_place.py delete mode 100644 crane_plus_examples_py/scripts/pick_and_place_tf.py delete mode 100644 crane_plus_examples_py/scripts/pose_groupstate.py delete mode 100644 crane_plus_examples_py/scripts/utils.py diff --git a/crane_plus_examples_py/scripts/aruco_detection.py b/crane_plus_examples_py/scripts/aruco_detection.py deleted file mode 100644 index 64734a5e..00000000 --- a/crane_plus_examples_py/scripts/aruco_detection.py +++ /dev/null @@ -1,116 +0,0 @@ -# Copyright 2020 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 rclpy -from rclpy.node import Node - -import numpy as np - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo, Image -import cv2 -from cv2 import aruco -from cv_bridge import CvBridge -import tf2_ros - -from utils import rotation_matrix_to_quaternion - - -class ImageSubscriber(Node): - def __init__(self): - super().__init__("aruco_detection") - self.image_subscription = self.create_subscription( - Image, "image_raw", self.listener_callback, 10 - ) - self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.listener_callback, 10 - ) - self.tf_broadcaster = tf2_ros.TransformBroadcaster() - - self.camera_info = None - self.bridge = CvBridge() - - def image_callback(self, msg): - # 画像データをROSのメッセージからOpenCVの配列に変換 - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) - cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) - - if self.camera_info: - # ArUcoマーカのデータセットを読み込む - # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの - MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) - - # マーカーID - ids = [] - - # 画像座標系上のマーカ頂点位置 - corners = [] - - # マーカの検出 - corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) - - # マーカの検出数 - n_markers = len(ids) - - # カメラパラメータ - CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) - DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) - - # マーカ一辺の長さ 0.04 [m] - MARKER_LENGTH = 0.04 - - # マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 - if n_markers > 0: - for i in range(n_markers): - # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 - rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) - # tfの配信 - t = TransformStamped() - t.header = msg.header - t.child_frame_id = "target_" + str(ids[i][0]) - t.transform.translation.x = tvecs[i][0][0] - t.transform.translation.y = tvecs[i][0][1] - t.transform.translation.z = tvecs[i][0][2] - # 回転ベクトル→回転行列 - rotation_matrix, _ = cv2.Rodrigues(rvecs[i]) - # 回転行列からクォータニオンを求める - q = rotation_matrix_to_quaternion(rotation_matrix) - t.transform.rotation.x = q.x - t.transform.rotation.y = q.y - t.transform.rotation.z = q.z - t.transform.rotation.w = q.w - self.tf_broadcaster.sendTransform(t) - - def camera_info_callback(self, msg): - self.camera_info = msg - - -def main(args=None): - rclpy.init(args=args) - - image_subscriber = ImageSubscriber() - rclpy.spin(image_subscriber) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - image_subscriber.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crane_plus_examples_py/scripts/color_detection.py b/crane_plus_examples_py/scripts/color_detection.py deleted file mode 100644 index 17ea59cf..00000000 --- a/crane_plus_examples_py/scripts/color_detection.py +++ /dev/null @@ -1,139 +0,0 @@ -# Copyright 2020 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 rclpy -from rclpy.node import Node - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo, Image -from image_geometry import PinholeCameraModel -import cv2 -from cv_bridge import CvBridge -import tf2_ros - - -class ImageSubscriber(Node): - def __init__(self): - super().__init__("color_detection") - self.image_subscription = self.create_subscription( - Image, "image_raw", self.listener_callback, 10 - ) - self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.listener_callback, 10 - ) - self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) - self.tf_broadcaster = tf2_ros.TransformBroadcaster() - self.camera_info = None - self.bridge = CvBridge() - - def image_callback(self, msg): - if self.camera_info: - # 赤い物体を検出するようにHSVの範囲を設定 - # 周囲の明るさ等の動作環境に合わせて調整 - LOW_H_1 = 0 - HIGH_H_1 = 20 - LOW_H_2 = 160 - HIGH_H_2 = 179 - LOW_S = 100 - HIGH_S = 255 - LOW_V = 50 - HIGH_V = 255 - - # ウェブカメラの画像を受け取る - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) - - # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） - cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) - - # 画像の二値化 - img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) - img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) - - # マスク画像の合成 - img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) - - # ノイズ除去の処理 - kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) - - # 穴埋めの処理 - kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) - - # 画像の検出領域におけるモーメントを計算 - moment = cv2.moments(img_thresholded) - d_m01 = moment['m01'] - d_m10 = moment['m10'] - d_area = moment['m00'] - - # 検出した領域のピクセル数が10000より大きい場合 - if d_area < 10000: - # カメラモデル作成 - camera_model = PinholeCameraModel() - - # カメラのパラメータを設定 - camera_model.fromCameraInfo(self.camera_info) - - # 画像座標系における把持対象物の位置（2D） - pixel_x = d_m10 / d_area - pixel_y = d_m01 / d_area - point = (pixel_x, pixel_y) - - # 補正後の画像座標系における把持対象物の位置を取得（2D） - rect_point = camera_model.rectifyImage(point) - - # カメラ座標系から見た把持対象物の方向（Ray）を取得する - ray = camera_model.projectPixelTo3dRay(rect_point) - - # カメラの高さを0.44[m]として把持対象物の位置を計算 - CAMERA_HEIGHT = 0.46 - object_position = { - 'x': ray.x * CAMERA_HEIGHT, - 'y': ray.y * CAMERA_HEIGHT, - 'z': ray.z * CAMERA_HEIGHT, - } - - # 把持対象物の位置をTFに配信 - t = TransformStamped() - t.header = msg.header - t.child_frame_id = "target_0" - t.transform.translation.x = object_position['x'] - t.transform.translation.y = object_position['y'] - t.transform.translation.z = object_position['z'] - self.tf_broadcaster.sendTransform(t) - - # 閾値による二値化画像を配信 - img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8") - self.image_thresholded_publisher.publish(img_thresholded_msg) - - def camera_info_callback(self, msg): - self.camera_info = msg - - -def main(args=None): - rclpy.init(args=args) - - image_subscriber = ImageSubscriber() - rclpy.spin(image_subscriber) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - image_subscriber.destroy_node() - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples_py/scripts/gripper_control.py b/crane_plus_examples_py/scripts/gripper_control.py deleted file mode 100644 index 3e771a3a..00000000 --- a/crane_plus_examples_py/scripts/gripper_control.py +++ /dev/null @@ -1,92 +0,0 @@ -# Copyright 2020 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 math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger - -# moveit python library -from moveit.core.robot_state import RobotState -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) -from utils import plan_and_execute - - -def main(args=None): - # ros2の初期化 - rclpy.init(args=args) - - # ロガー生成 - logger = get_logger("gripper_control") - - # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_gripper = crane_plus.get_planning_component("gripper") - logger.info("MoveItPy instance created") - - # instantiate a RobotState instance using the current robot model - robot_model = crane_plus.get_robot_model() - robot_state = RobotState(robot_model) - - # planningのパラメータ設定 - plan_request_params = PlanRequestParameters( - crane_plus, - "ompl_rrtc", - ) - plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 - - # グリッパーを+30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(30.0)]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=plan_request_params, - ) - # グリッパーを-30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(-30.0)]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=plan_request_params, - ) - # グリッパーを0[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [0.0]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=plan_request_params, - ) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crane_plus_examples_py/scripts/joint_values.py b/crane_plus_examples_py/scripts/joint_values.py deleted file mode 100644 index a8fb9d8f..00000000 --- a/crane_plus_examples_py/scripts/joint_values.py +++ /dev/null @@ -1,96 +0,0 @@ -# Copyright 2020 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 math - -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger - -# moveit python library -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) - -from utils import plan_and_execute - - -def to_radians(deg_angle): - return deg_angle * math.pi / 180.0 - - -def main(args=None): - # ros2の初期化 - rclpy.init(args=args) - - # ロガー生成 - logger = get_logger("joint_values") - - # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - logger.info("MoveItPy instance created") - - # 速度＆加速度のスケーリングファクタを設定 - plan_request_params = PlanRequestParameters( - crane_plus, - "ompl_rrtc", - ) - plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 - - # 現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # 現在の位置からarmの関節を全て順番に45度動かす - joint_values = crane_plus_arm.get_named_target_state_values("arm") - target_joint_value = math.radians(45.0) - - # joint_valuesはDict型なので、keyの数（=jointの数）だけ関節を動かす - for key in enumerate(joint_values.keys()): - joint_values[key] = target_joint_value - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # 現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # MoveItPyの終了 - crane_plus.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crane_plus_examples_py/scripts/pick_and_place.py b/crane_plus_examples_py/scripts/pick_and_place.py deleted file mode 100644 index e549f7d6..00000000 --- a/crane_plus_examples_py/scripts/pick_and_place.py +++ /dev/null @@ -1,275 +0,0 @@ -# Copyright 2020 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 math - -import rclpy -from geometry_msgs.msg import PoseStamped - -# generic ros libraries -from rclpy.logging import get_logger - -# moveit python library -from moveit.core.robot_state import RobotState -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) -from utils import plan_and_execute, euler_to_quaternion - - -def to_radians(deg_angle): - return deg_angle * math.pi / 180.0 - - -def main(args=None): - # ros2の初期化 - rclpy.init(args=args) - - # ロガー生成 - logger = get_logger("pick_and_place") - - # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - crane_plus_gripper = crane_plus.get_planning_component("gripper") - logger.info("MoveItPy instance created") - - # instantiate a RobotState instance using the current robot model - robot_model = crane_plus.get_robot_model() - robot_state = RobotState(robot_model) - - # planningのパラメータ設定 - # armのパラメータ設定用 - arm_plan_request_params = PlanRequestParameters( - crane_plus, - "ompl_rrtc", - ) - arm_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 - - # gripperのパラメータ設定用 - gripper_plan_request_params = PlanRequestParameters( - crane_plus, - "ompl_rrtc", - ) - gripper_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 - - # gripperの開閉角度 - GRIPPER_DEFAULT = 0.0 - GRIPPER_OPEN = math.radians(-30.0) - GRIPPER_CLOSE = math.radians(10.0) - - # armを現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # gripperを0度の位置に動かす - robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # ----- Picking Preparation ----- - # armを現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # なにか掴んでいたときのためにgripperを開く - robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # armが掴みに行く位置を指定して動かす - pose_goal = PoseStamped() - pose_goal.header.frame_id = "base_link" - pose_goal.pose.position.x = 0.0 - pose_goal.pose.position.y = -0.09 - pose_goal.pose.position.z = 0.17 - q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(-90)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # armの姿勢を変える(Y軸反転) - q = euler_to_quaternion(to_radians(0), to_radians(180), to_radians(-90)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # armをz軸上に動かす - pose_goal.pose.position.z = 0.14 - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # Grasp - robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # armをz軸上に動かす - pose_goal.pose.position.z = 0.17 - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # ----- Placing Preparation ----- - # armを現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # armが掴みに行く位置を指定して動かす - pose_goal.pose.position.x = 0.15 - pose_goal.pose.position.y = 0.0 - pose_goal.pose.position.z = 0.06 - q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(0)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # Release - robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - - # Return to home and vertical pose - # armを現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # armを現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # gripperの開閉をデフォルトの間隔に戻す - robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crane_plus_examples_py/scripts/pick_and_place_tf.py b/crane_plus_examples_py/scripts/pick_and_place_tf.py deleted file mode 100644 index 36d51d73..00000000 --- a/crane_plus_examples_py/scripts/pick_and_place_tf.py +++ /dev/null @@ -1,252 +0,0 @@ -# Copyright 2020 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 math -import numpy as np -import datetime - -# import angles -from geometry_msgs.msg import Pose -from moveit_msgs.msg import Constraints, JointConstraint -import rclpy -from rclpy.time import Time -from rclpy.node import Node -import tf2_ros -from tf2_ros import TransformStamped, TransformListener -from tf2_ros.buffer import Buffer - -# moveit python library -from moveit.core.robot_state import RobotState -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) - -from utils import plan_and_execute, euler_to_quaternion - - -class PickAndPlaceTf(Node): - def __init__(self, crane_plus): - super().__init__("pick_and_place_tf_node") - self.tf_past = TransformStamped() - self.crane_plus = crane_plus - self.crane_plus_arm = crane_plus.get_planning_component("arm") - self.crane_plus_gripper = crane_plus.get_planning_component("gripper") - - # instantiate a RobotState instance using the current robot model - self.robot_model = crane_plus.get_robot_model() - self.robot_state = RobotState(self.robot_model) - - # planningのパラメータ設定 - # armのパラメータ設定用 - self.arm_plan_request_params = PlanRequestParameters( - self.crane_plus, - "ompl_rrtc", - ) - self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - - # gripperのパラメータ設定用 - self.gripper_plan_request_params = PlanRequestParameters( - self.crane_plus, - "ompl_rrtc", - ) - self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - - # SRDFに定義されている"home"の姿勢にする - self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - self.crane_plus, - self.crane_plus_arm, - logger=None, - single_plan_parameters=self.arm_plan_request_params, - ) - - # 可動範囲を制限する - constraints = Constraints() - constraints.name = "arm_constraints" - - jointConstraint = JointConstraint() - jointConstraint.joint_name = "crane_plus_joint1" - jointConstraint.position = 0.0 - jointConstraint.tolerance_above = math.radians(100) - jointConstraint.tolerance_below = math.radians(100) - jointConstraint.weight = 1.0 - constraints.joint_constraints.append(jointConstraint) - - jointConstraint.joint_name = "crane_plus_joint3" - jointConstraint.position = 0.0 - jointConstraint.tolerance_above = math.radians(0) - jointConstraint.tolerance_below = math.radians(180) - jointConstraint.weight = 1.0 - constraints.joint_constraints.append(jointConstraint) - - self.crane_plus_arm.set_path_constraints(constraints) - - # 待機姿勢 - self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) - - # tf - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) - - # Call on_timer function every second - self.timer = self.create_timer(0.5, self.on_timer) - - def on_timer(self): - # target_0のtf位置姿勢を取得 - try: - tf_msg = self.tf_buffer.lookup_transform( - 'base_link', 'target_0', Time()) - except tf2_ros.LookupException as ex: - self.get_logger().info(f'Could not transform base_link to target: {ex}') - - now = Time() - FILTERING_TIME = datetime.timedelta(seconds=2) - STOP_TIME_THRESHOLD = datetime.timedelta(seconds=3) - DISTANCE_THRESHOLD = 0.01 - - # 経過時間と停止時間を計算(nsec) - # 経過時間 - TF_ELAPSED_TIME = now.nanoseconds - tf_msg.header.stamp.nanosec - # 停止時間 - if self.tf_past is not None: - TF_STOP_TIME = now.nanoseconds - self.tf_past.header.stamp.nanosec - else: - TF_STOP_TIME = now.nanoseconds - - # 現在時刻から2秒以内に受け取ったtfを使用 - if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: - tf_diff = np.sqrt((self.tf_past.transform.translation.x - - tf_msg.transform.translation.x) ** 2 - + (self.tf_past.transform.translation.y - - tf_msg.transform.translation.y) ** 2 - + (self.tf_past.transform.translation.z - - tf_msg.transform.translation.z) ** 2 - ) - # 把持対象の位置が停止していることを判定 - if tf_diff < DISTANCE_THRESHOLD: - # 把持対象が3秒以上停止している場合ピッキング動作開始 - if TF_STOP_TIME > STOP_TIME_THRESHOLD.total_seconds() * 1e9: - self._picking(tf_msg) - else: - self.tf_past = tf_msg - - def _picking(self, tf_msg): - GRIPPER_DEFAULT = 0.0 - GRIPPER_OPEN = math.radians(-30.0) - GRIPPER_CLOSE = math.radians(10.0) - - # 何かを掴んでいた時のためにハンドを開く - self._control_gripper(GRIPPER_OPEN) - - # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 - x = tf_msg.transform.translation.x - y = tf_msg.transform.translation.y - theta_rad = math.atan2(y, x) - theta_deg = math.degrees(theta_rad) - - # 把持対象物に正対する - self._control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg) - - # 掴みに行く - GRIPPER_OFFSET = 0.13 - gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) - gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) - if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, - 0.05, 0, 90, theta_deg): - # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る - self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) - return - - # ハンドを閉じる - self._control_gripper(GRIPPER_CLOSE) - - # 移動する - self._control_arm(0.0, 0.0, 0.17, 0, 90, 0) - - # 下ろす - self._control_arm(0.0, -0.15, 0.06, 0, 90, -90) - - # ハンドを開く - self._control_gripper(GRIPPER_OPEN) - - # 少しだけハンドを持ち上げる - self._control_arm(0.0, -0.15, 0.10, 0, 90, -90) - - # 待機姿勢に戻る - self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) - - # ハンドを閉じる - self._control_gripper(GRIPPER_DEFAULT) - - # グリッパ制御 - def _control_gripper(self, angle): - self.robot_state.set_joint_group_positions("gripper", [angle]) - self.crane_plus_gripper.set_start_state_to_current_state() - self.crane_plus_gripper.set_goal_state(robot_state=self.robot_state) - plan_and_execute( - self.crane_plus, - self.crane_plus_gripper, - logger=None, - single_plan_parameters=self.gripper_plan_request_params, - ) - - # アーム制御 - def _control_arm(self, x, y, z, roll, pitch, yaw): - target_pose = Pose() - target_pose.position.x = x - target_pose.position.y = y - target_pose.position.z = z - q = euler_to_quaternion(math.radians(roll), math.radians(pitch), - math.radians(yaw)) - target_pose.orientation.x = q[0] - target_pose.orientation.y = q[1] - target_pose.orientation.z = q[2] - target_pose.orientation.w = q[3] - self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link="crane_plus_link4" - ) - result = plan_and_execute( - self.crane_plus, - self.crane_plus_arm, - logger=None, - single_plan_parameters=self.arm_plan_request_params, - ) - return result - - -def main(args=None): - # ros2の初期化 - rclpy.init(args=args) - - # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - - # node生成 - pick_and_place_tf_node = PickAndPlaceTf(crane_plus) - rclpy.spin(pick_and_place_tf_node) - - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 - rclpy.shutdown() - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/crane_plus_examples_py/scripts/pose_groupstate.py b/crane_plus_examples_py/scripts/pose_groupstate.py deleted file mode 100644 index f3fc13bd..00000000 --- a/crane_plus_examples_py/scripts/pose_groupstate.py +++ /dev/null @@ -1,84 +0,0 @@ - -# Copyright 2020 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 rclpy - -# generic ros libraries -from rclpy.logging import get_logger - -# moveit python library -from moveit.planning import ( - MoveItPy, - PlanRequestParameters, -) -from utils import plan_and_execute - - -def main(args=None): - # ros2の初期化 - rclpy.init(args=args) - - # ロガー生成 - logger = get_logger("pose_groupstate") - - # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - logger.info("MoveItPy instance created") - - # planningのパラメータ設定 - plan_request_params = PlanRequestParameters( - crane_plus, - "ompl_rrtc", - ) - plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - - # 現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # 現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # 現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # MoveItPyの終了 - crane_plus.shutdown() - - -if __name__ == "__main__": - main() diff --git a/crane_plus_examples_py/scripts/utils.py b/crane_plus_examples_py/scripts/utils.py deleted file mode 100644 index 6ad5c653..00000000 --- a/crane_plus_examples_py/scripts/utils.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright 2020 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 time -import numpy as np -import quaternion - - -# plan and execute -def plan_and_execute( - robot, - planning_component, - logger, - single_plan_parameters=None, - multi_plan_parameters=None, - sleep_time=0.0, -): - """Helper function to plan and execute a motion.""" - # plan to goal - logger.info("Planning trajectory") - if multi_plan_parameters is not None: - plan_result = planning_component.plan( - multi_plan_parameters=multi_plan_parameters - ) - elif single_plan_parameters is not None: - plan_result = planning_component.plan( - single_plan_parameters=single_plan_parameters - ) - else: - plan_result = planning_component.plan() - - # execute the plan - result = None - if plan_result: - logger.info("Executing plan") - robot_trajectory = plan_result.trajectory - result = robot.execute(robot_trajectory, controllers=[]) - else: - logger.error("Planning failed") - result = False - time.sleep(sleep_time) - return result - - -# euler --> quaternion -def euler_to_quaternion(roll, pitch, yaw): - cy = np.cos(yaw * 0.5) - sy = np.sin(yaw * 0.5) - cr = np.cos(roll * 0.5) - sr = np.sin(roll * 0.5) - cp = np.cos(pitch * 0.5) - sp = np.sin(pitch * 0.5) - - qw = cy * cr * cp + sy * sr * sp - qx = cy * sr * cp - sy * cr * sp - qy = cy * cr * sp + sy * sr * cp - qz = sy * cr * cp - cy * sr * sp - - return [qx, qy, qz, qw] - - -# rotation matrix --> quaternion -def rotation_matrix_to_quaternion(rotation_matrix): - # numpy-quaternionを使用して回転行列からクォータニオンを計算 - #3x3の回転行列をnumpy.quaternionに変換する - q = quaternion.from_rotation_matrix(rotation_matrix) - return q \ No newline at end of file From 9277ab5062e1666572cf7d2dff85a30f746584aa Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 20 Sep 2024 18:22:44 +0900 Subject: [PATCH 030/111] =?UTF-8?q?numpy-quaternion-pip=E3=82=92package.xm?= =?UTF-8?q?l=E3=81=AB=E5=90=AB=E3=82=81=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index de45bb77..aa728a44 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -11,6 +11,7 @@ std_msgs moveit moveit_py + python-numpy-quaternion-pip ament_copyright ament_flake8 From 70deeb6d05846b244777350c4cd051fa60fd0ae9 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Wed, 25 Sep 2024 10:27:28 +0900 Subject: [PATCH 031/111] =?UTF-8?q?package.xml=E3=81=ABpython3-pip?= =?UTF-8?q?=E3=81=AE=E4=BE=9D=E5=AD=98=E9=96=A2=E4=BF=82=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index aa728a44..a44f9610 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -11,8 +11,8 @@ std_msgs moveit moveit_py + python3-pip python-numpy-quaternion-pip - ament_copyright ament_flake8 ament_pep257 From 094fa3cb17e2f63975d66bd47c160a0249df9c92 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Thu, 26 Sep 2024 17:53:59 +0900 Subject: [PATCH 032/111] =?UTF-8?q?pose=5Fgroupstate.py=E3=81=A8gripper=5F?= =?UTF-8?q?control.py=E3=81=AB=E3=81=A4=E3=81=84=E3=81=A6=E5=8B=95?= =?UTF-8?q?=E4=BD=9C=E7=A2=BA=E8=AA=8D=E3=81=A7=E3=81=8D=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 14 ++++++++------ .../crane_plus_examples_py/pose_groupstate.py | 2 +- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 3e771a3a..1213e7da 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -24,13 +24,13 @@ MoveItPy, PlanRequestParameters, ) -from utils import plan_and_execute + +from crane_plus_examples_py.utils import plan_and_execute def main(args=None): # ros2の初期化 rclpy.init(args=args) - # ロガー生成 logger = get_logger("gripper_control") @@ -48,11 +48,12 @@ def main(args=None): crane_plus, "ompl_rrtc", ) - plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 + + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # グリッパーを+30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(30.0)]) + robot_state.set_joint_group_positions("gripper", [math.radians(30)]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -62,7 +63,7 @@ def main(args=None): single_plan_parameters=plan_request_params, ) # グリッパーを-30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(-30.0)]) + robot_state.set_joint_group_positions("gripper", [math.radians(-30)]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -81,6 +82,7 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) + # MoveItPyの終了 crane_plus.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index f3fc13bd..1464e2fc 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -23,7 +23,7 @@ MoveItPy, PlanRequestParameters, ) -from utils import plan_and_execute +from crane_plus_examples_py.utils import plan_and_execute def main(args=None): From d0c1f6763587092575710bdab24414c3f90c9176 Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Fri, 27 Sep 2024 16:18:30 +0900 Subject: [PATCH 033/111] =?UTF-8?q?gripper=5Fcontrol.py=E3=81=A8pose=5Fgro?= =?UTF-8?q?upstate.py=E3=81=AB=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 1 + .../crane_plus_examples_py/pose_groupstate.py | 30 +++++++++++-------- 2 files changed, 18 insertions(+), 13 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 1213e7da..d1b351bf 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -49,6 +49,7 @@ def main(args=None): "ompl_rrtc", ) + # 速度＆加速度のスケーリングファクタを設定 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index 1464e2fc..4354a7df 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -1,12 +1,11 @@ - # Copyright 2020 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. @@ -43,6 +42,8 @@ def main(args=None): crane_plus, "ompl_rrtc", ) + + # 速度＆加速度のスケーリングファクタを設定 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 @@ -50,9 +51,9 @@ def main(args=None): crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state(configuration_name="home") plan_and_execute( - crane_plus, - crane_plus_arm, - logger, + crane_plus, + crane_plus_arm, + logger, single_plan_parameters=plan_request_params, ) @@ -60,9 +61,9 @@ def main(args=None): crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state(configuration_name="vertical") plan_and_execute( - crane_plus, - crane_plus_arm, - logger, + crane_plus, + crane_plus_arm, + logger, single_plan_parameters=plan_request_params, ) @@ -70,15 +71,18 @@ def main(args=None): crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state(configuration_name="home") plan_and_execute( - crane_plus, - crane_plus_arm, - logger, + crane_plus, + crane_plus_arm, + logger, single_plan_parameters=plan_request_params, ) # MoveItPyの終了 crane_plus.shutdown() + # rclpyの終了 + rclpy.shutdown() + if __name__ == "__main__": main() From ccfb5444d11e272943ed83c2a0315091f8f6fa4b Mon Sep 17 00:00:00 2001 From: Nozomi Mizoguchi Date: Mon, 30 Sep 2024 13:31:42 +0900 Subject: [PATCH 034/111] =?UTF-8?q?joint=5Fvalues.py=E3=81=AB=E3=81=A4?= =?UTF-8?q?=E3=81=84=E3=81=A6=E5=8B=95=E4=BD=9C=E7=A2=BA=E8=AA=8D=E3=81=A7?= =?UTF-8?q?=E3=81=8D=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/joint_values.py | 55 +++++++++---------- 1 file changed, 25 insertions(+), 30 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index a8fb9d8f..6e1cd289 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -1,11 +1,11 @@ # Copyright 2020 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. @@ -20,16 +20,14 @@ from rclpy.logging import get_logger # moveit python library +from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) +from moveit.core.kinematic_constraints import construct_joint_constraint -from utils import plan_and_execute - - -def to_radians(deg_angle): - return deg_angle * math.pi / 180.0 +from crane_plus_examples_py.utils import plan_and_execute def main(args=None): @@ -43,41 +41,35 @@ def main(args=None): crane_plus = MoveItPy(node_name="moveit_py") crane_plus_arm = crane_plus.get_planning_component("arm") logger.info("MoveItPy instance created") + + # instantiate a a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) - # 速度＆加速度のスケーリングファクタを設定 plan_request_params = PlanRequestParameters( crane_plus, "ompl_rrtc", ) - plan_request_params.max_acceleration_scaling_factor(1.0) # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor(1.0) # Set 0.0 ~ 1.0 - - # 現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=plan_request_params, - ) - - # 現在の位置からarmの関節を全て順番に45度動かす - joint_values = crane_plus_arm.get_named_target_state_values("arm") - target_joint_value = math.radians(45.0) - # joint_valuesはDict型なので、keyの数（=jointの数）だけ関節を動かす - for key in enumerate(joint_values.keys()): - joint_values[key] = target_joint_value + # armの関節のjoint1〜4を順番に45[deg]ずつ動かす + joint_names = ["crane_plus_joint1", "crane_plus_joint2", "crane_plus_joint3", "crane_plus_joint4"] + target_joint_value = math.radians(45) + for joint_name in joint_names: + joint_values = {joint_name: target_joint_value} + robot_state.joint_positions = joint_values + joint_constraint = construct_joint_constraint( + robot_state=robot_state, + joint_model_group=crane_plus.get_robot_model().get_joint_model_group("arm"), + ) crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(motion_plan_constraints=[joint_constraint]) plan_and_execute( crane_plus, crane_plus_arm, logger, single_plan_parameters=plan_request_params, ) - + # 現在の位置から"vertical"ポジションに動かす crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state(configuration_name="vertical") @@ -91,6 +83,9 @@ def main(args=None): # MoveItPyの終了 crane_plus.shutdown() + # rclpyの終了 + rclpy.shutdown() + if __name__ == "__main__": main() From 1c3b113978e397513c4a1afc7d1d8cb64e4e9955 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 4 Oct 2024 11:30:49 +0900 Subject: [PATCH 035/111] =?UTF-8?q?=E3=83=AC=E3=83=93=E3=83=A5=E3=83=BC?= =?UTF-8?q?=E7=B5=90=E6=9E=9C=E5=8F=8D=E6=98=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 3 +- crane_plus_examples/launch/demo.launch.py | 2 +- .../launch/run_move_group.launch.py | 36 +++++-------------- crane_plus_moveit_config/package.xml | 2 +- 4 files changed, 12 insertions(+), 31 deletions(-) diff --git a/.gitignore b/.gitignore index 04d4c3a6..2152342c 100755 --- a/.gitignore +++ b/.gitignore @@ -55,5 +55,4 @@ docs/_build/ # PyBuilder target/ -.pytest_cache -.vscode +.pytest_cache \ No newline at end of file diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 17cd579c..12a7dbeb 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): default_value='/dev/ttyUSB0', description='Set port name.' ) - + declare_use_camera = DeclareLaunchArgument( 'use_camera', default_value='false', 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 e20593ca..c51d7ad4 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -26,31 +26,6 @@ from moveit_configs_utils.launches import generate_rsp_launch from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg -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(): ld = LaunchDescription() @@ -74,7 +49,15 @@ def generate_launch_description(): description="Set the path to rviz configuration file.", ) ) - + + declare_rviz_config_file = DeclareLaunchArgument( + 'rviz_config_file', + default_value=get_package_share_directory('crane_plus_moveit_config') + '/launch/run_move_group.rviz', + description='Set the path to rviz configuration file.' + ) + + ld.add_action(declare_rviz_config_file) + moveit_config = ( MoveItConfigsBuilder("crane_plus") .planning_scene_monitor( @@ -112,7 +95,6 @@ def generate_launch_description(): # Move group ld.add_entity(generate_move_group_launch(moveit_config)) - # RViz ld.add_entity(generate_moveit_rviz_launch(moveit_config)) diff --git a/crane_plus_moveit_config/package.xml b/crane_plus_moveit_config/package.xml index 879c2211..df03651c 100755 --- a/crane_plus_moveit_config/package.xml +++ b/crane_plus_moveit_config/package.xml @@ -39,7 +39,7 @@ ament_lint_auto ament_lint_common - + ament_cmake From bb49b1e34182228a8a1b8d64c1ba8f7ef5396592 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 4 Oct 2024 18:11:53 +0900 Subject: [PATCH 036/111] =?UTF-8?q?radian=E3=81=AE=E8=A8=88=E7=AE=97?= =?UTF-8?q?=E3=81=AFpython=E3=81=AEmath=E3=82=92=E4=BD=BF=E3=81=86?= =?UTF-8?q?=E3=81=93=E3=81=A8=E3=81=AB=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place.py | 25 ++++++++++--------- 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index e549f7d6..dba06cb0 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -26,11 +26,8 @@ MoveItPy, PlanRequestParameters, ) -from utils import plan_and_execute, euler_to_quaternion - -def to_radians(deg_angle): - return deg_angle * math.pi / 180.0 +from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion def main(args=None): @@ -56,16 +53,16 @@ def main(args=None): crane_plus, "ompl_rrtc", ) - arm_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # gripperのパラメータ設定用 gripper_plan_request_params = PlanRequestParameters( crane_plus, "ompl_rrtc", ) - gripper_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # gripperの開閉角度 GRIPPER_DEFAULT = 0.0 @@ -117,15 +114,18 @@ def main(args=None): # armが掴みに行く位置を指定して動かす pose_goal = PoseStamped() - pose_goal.header.frame_id = "base_link" + pose_goal.header.frame_id = "world" + # pose_goal.header.frame_id = "base_link" + # pose_goal.header.frame_id = "crane_plus_base" pose_goal.pose.position.x = 0.0 pose_goal.pose.position.y = -0.09 pose_goal.pose.position.z = 0.17 - q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(-90)) + q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) pose_goal.pose.orientation.x = q[0] pose_goal.pose.orientation.y = q[1] pose_goal.pose.orientation.z = q[2] pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" @@ -138,7 +138,7 @@ def main(args=None): ) # armの姿勢を変える(Y軸反転) - q = euler_to_quaternion(to_radians(0), to_radians(180), to_radians(-90)) + q = euler_to_quaternion(math.radians(0), math.radians(180), math.radians(-90)) pose_goal.pose.orientation.x = q[0] pose_goal.pose.orientation.y = q[1] pose_goal.pose.orientation.z = q[2] @@ -206,7 +206,7 @@ def main(args=None): pose_goal.pose.position.x = 0.15 pose_goal.pose.position.y = 0.0 pose_goal.pose.position.z = 0.06 - q = euler_to_quaternion(to_radians(0), to_radians(90), to_radians(0)) + q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(0)) pose_goal.pose.orientation.x = q[0] pose_goal.pose.orientation.y = q[1] pose_goal.pose.orientation.z = q[2] @@ -264,6 +264,7 @@ def main(args=None): logger, single_plan_parameters=gripper_plan_request_params, ) + # MoveItPyの終了 crane_plus.shutdown() From e46d557dc219c7bc9a0689f5f239601d4f7acec3 Mon Sep 17 00:00:00 2001 From: mizonon Date: Wed, 16 Oct 2024 11:21:03 +0900 Subject: [PATCH 037/111] =?UTF-8?q?utils.py=E3=81=AA=E3=81=A9=E3=81=AE?= =?UTF-8?q?=E7=B4=B0=E3=81=8B=E3=81=84=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/crane_plus_moveit_py_examples.yaml | 5 +- .../crane_plus_examples_py/aruco_detection.py | 2 +- .../crane_plus_examples_py/pick_and_place.py | 331 +++++++++--------- .../pick_and_place_tf.py | 14 +- .../crane_plus_examples_py/utils.py | 7 + .../launch/example.launch.py | 45 ++- 6 files changed, 218 insertions(+), 186 deletions(-) diff --git a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml index c439aef7..6b168475 100644 --- a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml +++ b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml @@ -20,7 +20,8 @@ ompl_rrtc: # Namespace for individual plan request plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem planning_pipeline: ompl # Name of the pipeline that is being used - planner_id: "RRTConnectkConfigDefault" # Name of the specific planner to be used by the pipeline + planner_id: "RRTConnectConfigDefault" # Name of the specific planner to be used by the pipeline max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning - planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned + # planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned + planning_time: 5.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 64734a5e..c3acd628 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -26,7 +26,7 @@ from cv_bridge import CvBridge import tf2_ros -from utils import rotation_matrix_to_quaternion +from crane_plus_examples_py.utils import rotation_matrix_to_quaternion class ImageSubscriber(Node): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index dba06cb0..9e12b963 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -79,52 +79,65 @@ def main(args=None): single_plan_parameters=arm_plan_request_params, ) - # gripperを0度の位置に動かす - robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) + # # gripperを0度の位置に動かす + # robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + # crane_plus_gripper.set_start_state_to_current_state() + # crane_plus_gripper.set_goal_state(robot_state=robot_state) + # plan_and_execute( + # crane_plus, + # crane_plus_gripper, + # logger, + # single_plan_parameters=gripper_plan_request_params, + # ) - # ----- Picking Preparation ----- - # armを現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # ----- Picking Preparation ----- + # # armを現在の位置から"home"ポジションに動かす + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state(configuration_name="home") + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # なにか掴んでいたときのためにgripperを開く - robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) + # # なにか掴んでいたときのためにgripperを開く + # robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + # crane_plus_gripper.set_start_state_to_current_state() + # crane_plus_gripper.set_goal_state(robot_state=robot_state) + # plan_and_execute( + # crane_plus, + # crane_plus_gripper, + # logger, + # single_plan_parameters=gripper_plan_request_params, + # ) - # armが掴みに行く位置を指定して動かす + # # armが掴みに行く位置を指定して動かす pose_goal = PoseStamped() - pose_goal.header.frame_id = "world" + # pose_goal.header.frame_id = "world" # pose_goal.header.frame_id = "base_link" - # pose_goal.header.frame_id = "crane_plus_base" + pose_goal.header.frame_id = "crane_plus_base" + + # # pose_goal.pose.position.x = 0.0 + # # pose_goal.pose.position.y = -0.09 + # # pose_goal.pose.position.z = 0.17 + # # q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) + # # pose_goal.pose.orientation.x = q[0] + # # pose_goal.pose.orientation.y = q[1] + # # pose_goal.pose.orientation.z = q[2] + # # pose_goal.pose.orientation.w = q[3] + + # # test >>> + # current_pose = crane_plus_arm.get_start_state().get_pose("crane_plus_link4") pose_goal.pose.position.x = 0.0 - pose_goal.pose.position.y = -0.09 - pose_goal.pose.position.z = 0.17 - q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.23 + # q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) + pose_goal.pose.orientation.x = 0.0 + pose_goal.pose.orientation.y = 0.0 + pose_goal.pose.orientation.z = 0.0 + pose_goal.pose.orientation.w = 1.0 + # <<< test crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( @@ -134,136 +147,136 @@ def main(args=None): crane_plus, crane_plus_arm, logger, - single_plan_parameters=arm_plan_request_params, + # single_plan_parameters=arm_plan_request_params, ) - # armの姿勢を変える(Y軸反転) - q = euler_to_quaternion(math.radians(0), math.radians(180), math.radians(-90)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # armの姿勢を変える(Y軸反転) + # q = euler_to_quaternion(math.radians(0), math.radians(180), math.radians(-90)) + # pose_goal.pose.orientation.x = q[0] + # pose_goal.pose.orientation.y = q[1] + # pose_goal.pose.orientation.z = q[2] + # pose_goal.pose.orientation.w = q[3] + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state( + # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + # ) + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # armをz軸上に動かす - pose_goal.pose.position.z = 0.14 - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # armをz軸上に動かす + # pose_goal.pose.position.z = 0.14 + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state( + # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + # ) + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # Grasp - robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) + # # Grasp + # robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) + # crane_plus_gripper.set_start_state_to_current_state() + # crane_plus_gripper.set_goal_state(robot_state=robot_state) + # plan_and_execute( + # crane_plus, + # crane_plus_gripper, + # logger, + # single_plan_parameters=gripper_plan_request_params, + # ) - # armをz軸上に動かす - pose_goal.pose.position.z = 0.17 - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # armをz軸上に動かす + # pose_goal.pose.position.z = 0.17 + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state( + # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + # ) + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # ----- Placing Preparation ----- - # armを現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # ----- Placing Preparation ----- + # # armを現在の位置から"home"ポジションに動かす + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state(configuration_name="home") + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # armが掴みに行く位置を指定して動かす - pose_goal.pose.position.x = 0.15 - pose_goal.pose.position.y = 0.0 - pose_goal.pose.position.z = 0.06 - q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(0)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # armが掴みに行く位置を指定して動かす + # pose_goal.pose.position.x = 0.15 + # pose_goal.pose.position.y = 0.0 + # pose_goal.pose.position.z = 0.06 + # q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(0)) + # pose_goal.pose.orientation.x = q[0] + # pose_goal.pose.orientation.y = q[1] + # pose_goal.pose.orientation.z = q[2] + # pose_goal.pose.orientation.w = q[3] + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state( + # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + # ) + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # Release - robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) + # # Release + # robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + # crane_plus_gripper.set_start_state_to_current_state() + # crane_plus_gripper.set_goal_state(robot_state=robot_state) + # plan_and_execute( + # crane_plus, + # crane_plus_gripper, + # logger, + # single_plan_parameters=gripper_plan_request_params, + # ) - # Return to home and vertical pose - # armを現在の位置から"home"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # Return to home and vertical pose + # # armを現在の位置から"home"ポジションに動かす + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state(configuration_name="home") + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # armを現在の位置から"vertical"ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) + # # armを現在の位置から"vertical"ポジションに動かす + # crane_plus_arm.set_start_state_to_current_state() + # crane_plus_arm.set_goal_state(configuration_name="vertical") + # plan_and_execute( + # crane_plus, + # crane_plus_arm, + # logger, + # single_plan_parameters=arm_plan_request_params, + # ) - # gripperの開閉をデフォルトの間隔に戻す - robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) - plan_and_execute( - crane_plus, - crane_plus_gripper, - logger, - single_plan_parameters=gripper_plan_request_params, - ) + # # gripperの開閉をデフォルトの間隔に戻す + # robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + # crane_plus_gripper.set_start_state_to_current_state() + # crane_plus_gripper.set_goal_state(robot_state=robot_state) + # plan_and_execute( + # crane_plus, + # crane_plus_gripper, + # logger, + # single_plan_parameters=gripper_plan_request_params, + # ) # MoveItPyの終了 crane_plus.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 36d51d73..d0ea07d8 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -22,6 +22,7 @@ import rclpy from rclpy.time import Time from rclpy.node import Node +from rclpy.logging import get_logger import tf2_ros from tf2_ros import TransformStamped, TransformListener from tf2_ros.buffer import Buffer @@ -33,17 +34,17 @@ PlanRequestParameters, ) -from utils import plan_and_execute, euler_to_quaternion +from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion class PickAndPlaceTf(Node): def __init__(self, crane_plus): super().__init__("pick_and_place_tf_node") + self.logger = get_logger("pick_and_place_tf") self.tf_past = TransformStamped() self.crane_plus = crane_plus self.crane_plus_arm = crane_plus.get_planning_component("arm") self.crane_plus_gripper = crane_plus.get_planning_component("gripper") - # instantiate a RobotState instance using the current robot model self.robot_model = crane_plus.get_robot_model() self.robot_state = RobotState(self.robot_model) @@ -71,7 +72,8 @@ def __init__(self, crane_plus): plan_and_execute( self.crane_plus, self.crane_plus_arm, - logger=None, + # logger=None, + self.logger, single_plan_parameters=self.arm_plan_request_params, ) @@ -201,7 +203,8 @@ def _control_gripper(self, angle): plan_and_execute( self.crane_plus, self.crane_plus_gripper, - logger=None, + # logger=None, + self.logger, single_plan_parameters=self.gripper_plan_request_params, ) @@ -224,7 +227,8 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): result = plan_and_execute( self.crane_plus, self.crane_plus_arm, - logger=None, + # logger=None, + self.logger, single_plan_parameters=self.arm_plan_request_params, ) return result diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 6ad5c653..56709c9a 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -16,6 +16,9 @@ import numpy as np import quaternion +# generic ros libraries +from rclpy.logging import get_logger + # plan and execute def plan_and_execute( @@ -28,17 +31,21 @@ def plan_and_execute( ): """Helper function to plan and execute a motion.""" # plan to goal + logger = get_logger("plan_and_execute") logger.info("Planning trajectory") if multi_plan_parameters is not None: plan_result = planning_component.plan( multi_plan_parameters=multi_plan_parameters ) + logger.info('plan_result >>>>> multi', once=True) elif single_plan_parameters is not None: plan_result = planning_component.plan( single_plan_parameters=single_plan_parameters ) + logger.info('plan_result >>>>> single', once=True) else: plan_result = planning_component.plan() + logger.info('plan_result >>>>> none', once=True) # execute the plan result = None diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index 7c558ff3..e5339656 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -35,34 +35,41 @@ def generate_launch_description(): description_loader = RobotDescriptionLoader() ld.add_action( DeclareLaunchArgument( - 'loaded_description', + "loaded_description", default_value=description_loader.load(), description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description." + It is recommended to use RobotDescriptionLoader() in crane_plus_description.", ) ) - + moveit_config = ( MoveItConfigsBuilder("crane_plus") .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) - # .robot_description( - # file_path=os.path.join( - # get_package_share_directory("crane_plus_description"), - # "urdf", - # "crane_plus.urdf.xacro", - # ), - # mappings={}, - # ) + .robot_description( + file_path=os.path.join( + get_package_share_directory("crane_plus_description"), + "urdf", + "crane_plus.urdf.xacro", + ), + mappings={}, + ) + .robot_description_kinematics( + file_path=get_package_share_directory("crane_plus_moveit_config") + + "/config/kinematics.yaml" + ) # .robot_description_semantic( - # file_path="config/crane_plus.srdf", + # file_path="config/crsane_plus.srdf", # mappings={"model": "crane_plus"}, # ) # .joint_limits(file_path="config/joint_limits.yaml") - .trajectory_execution(file_path="config/controllers.yaml") - .planning_pipelines(pipelines=["ompl"]) + .trajectory_execution( + file_path=get_package_share_directory("crane_plus_moveit_config") + + "/config/controllers.yaml" + ) + # .planning_pipelines(pipelines=["ompl"]) .moveit_cpp( file_path=get_package_share_directory("crane_plus_examples_py") + "/config/crane_plus_moveit_py_examples.yaml" @@ -77,11 +84,11 @@ def generate_launch_description(): moveit_config.move_group_capabilities = {"capabilities": ""} declare_example_name = DeclareLaunchArgument( - 'example', - default_value='gripper_control', + "example", + default_value="gripper_control", description=( - 'Set an example executable name: ' - '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' + "Set an example executable name: " + "[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]" ), ) @@ -102,7 +109,7 @@ def generate_launch_description(): output="screen", parameters=[moveit_config.to_dict()], ) - + # ld = LaunchDescription([SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time'))]) # ld.add_action(declare_use_sim_time) # ld.add_action(use_sim_time_name) From 2ea61a064421cfe058ad8c88a4c896ae56b5e3b0 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 18 Oct 2024 17:12:52 +0900 Subject: [PATCH 038/111] =?UTF-8?q?moveit2=E3=82=92=E3=82=BD=E3=83=BC?= =?UTF-8?q?=E3=82=B9=E3=81=8B=E3=82=89=E3=83=93=E3=83=AB=E3=83=89=E3=81=97?= =?UTF-8?q?=E3=81=A6=E5=8B=95=E3=81=84=E3=81=9F=E3=81=A8=E3=81=8D=E3=81=AE?= =?UTF-8?q?=E3=83=97=E3=83=AD=E3=82=B0=E3=83=A9=E3=83=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/crane_plus_moveit_py_examples.yaml | 3 +- .../crane_plus_examples_py/pick_and_place.py | 330 +++++++++--------- 2 files changed, 159 insertions(+), 174 deletions(-) diff --git a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml index 6b168475..bf3489ee 100644 --- a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml +++ b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml @@ -20,8 +20,7 @@ ompl_rrtc: # Namespace for individual plan request plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem planning_pipeline: ompl # Name of the pipeline that is being used - planner_id: "RRTConnectConfigDefault" # Name of the specific planner to be used by the pipeline + planner_id: "RRTConnectkConfigDefault:" # Name of the specific planner to be used by the pipeline max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning - # planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned planning_time: 5.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 9e12b963..6cbd16ae 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -79,66 +79,52 @@ def main(args=None): single_plan_parameters=arm_plan_request_params, ) - # # gripperを0度の位置に動かす - # robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) - # crane_plus_gripper.set_start_state_to_current_state() - # crane_plus_gripper.set_goal_state(robot_state=robot_state) - # plan_and_execute( - # crane_plus, - # crane_plus_gripper, - # logger, - # single_plan_parameters=gripper_plan_request_params, - # ) + # gripperを0度の位置に動かす + robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) - # # ----- Picking Preparation ----- - # # armを現在の位置から"home"ポジションに動かす - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state(configuration_name="home") - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # ----- Picking Preparation ----- + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # なにか掴んでいたときのためにgripperを開く - # robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) - # crane_plus_gripper.set_start_state_to_current_state() - # crane_plus_gripper.set_goal_state(robot_state=robot_state) - # plan_and_execute( - # crane_plus, - # crane_plus_gripper, - # logger, - # single_plan_parameters=gripper_plan_request_params, - # ) + # なにか掴んでいたときのためにgripperを開く + robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) - # # armが掴みに行く位置を指定して動かす + # armが掴みに行く位置を指定して動かす pose_goal = PoseStamped() - # pose_goal.header.frame_id = "world" - # pose_goal.header.frame_id = "base_link" pose_goal.header.frame_id = "crane_plus_base" - # # pose_goal.pose.position.x = 0.0 - # # pose_goal.pose.position.y = -0.09 - # # pose_goal.pose.position.z = 0.17 - # # q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) - # # pose_goal.pose.orientation.x = q[0] - # # pose_goal.pose.orientation.y = q[1] - # # pose_goal.pose.orientation.z = q[2] - # # pose_goal.pose.orientation.w = q[3] - - # # test >>> - # current_pose = crane_plus_arm.get_start_state().get_pose("crane_plus_link4") pose_goal.pose.position.x = 0.0 - pose_goal.pose.position.y = 0.0 - pose_goal.pose.position.z = 0.23 - # q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) - pose_goal.pose.orientation.x = 0.0 - pose_goal.pose.orientation.y = 0.0 - pose_goal.pose.orientation.z = 0.0 - pose_goal.pose.orientation.w = 1.0 - # <<< test - + pose_goal.pose.position.y = -0.09 + pose_goal.pose.position.z = 0.17 + q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" @@ -147,136 +133,136 @@ def main(args=None): crane_plus, crane_plus_arm, logger, - # single_plan_parameters=arm_plan_request_params, + single_plan_parameters=arm_plan_request_params, ) - # # armの姿勢を変える(Y軸反転) - # q = euler_to_quaternion(math.radians(0), math.radians(180), math.radians(-90)) - # pose_goal.pose.orientation.x = q[0] - # pose_goal.pose.orientation.y = q[1] - # pose_goal.pose.orientation.z = q[2] - # pose_goal.pose.orientation.w = q[3] - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state( - # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - # ) - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # armの姿勢を変える(Y軸反転) + q = euler_to_quaternion(math.radians(0), math.radians(180), math.radians(-90)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # armをz軸上に動かす - # pose_goal.pose.position.z = 0.14 - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state( - # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - # ) - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # armをz軸上に動かす + pose_goal.pose.position.z = 0.14 + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # Grasp - # robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) - # crane_plus_gripper.set_start_state_to_current_state() - # crane_plus_gripper.set_goal_state(robot_state=robot_state) - # plan_and_execute( - # crane_plus, - # crane_plus_gripper, - # logger, - # single_plan_parameters=gripper_plan_request_params, - # ) + # Grasp + robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) - # # armをz軸上に動かす - # pose_goal.pose.position.z = 0.17 - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state( - # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - # ) - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # armをz軸上に動かす + pose_goal.pose.position.z = 0.17 + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # ----- Placing Preparation ----- - # # armを現在の位置から"home"ポジションに動かす - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state(configuration_name="home") - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # ----- Placing Preparation ----- + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # armが掴みに行く位置を指定して動かす - # pose_goal.pose.position.x = 0.15 - # pose_goal.pose.position.y = 0.0 - # pose_goal.pose.position.z = 0.06 - # q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(0)) - # pose_goal.pose.orientation.x = q[0] - # pose_goal.pose.orientation.y = q[1] - # pose_goal.pose.orientation.z = q[2] - # pose_goal.pose.orientation.w = q[3] - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state( - # pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" - # ) - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # armが掴みに行く位置を指定して動かす + pose_goal.pose.position.x = 0.15 + pose_goal.pose.position.y = 0.0 + pose_goal.pose.position.z = 0.06 + q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(0)) + pose_goal.pose.orientation.x = q[0] + pose_goal.pose.orientation.y = q[1] + pose_goal.pose.orientation.z = q[2] + pose_goal.pose.orientation.w = q[3] + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state( + pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + ) + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # Release - # robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) - # crane_plus_gripper.set_start_state_to_current_state() - # crane_plus_gripper.set_goal_state(robot_state=robot_state) - # plan_and_execute( - # crane_plus, - # crane_plus_gripper, - # logger, - # single_plan_parameters=gripper_plan_request_params, - # ) + # Release + robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) - # # Return to home and vertical pose - # # armを現在の位置から"home"ポジションに動かす - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state(configuration_name="home") - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # Return to home and vertical pose + # armを現在の位置から"home"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="home") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # armを現在の位置から"vertical"ポジションに動かす - # crane_plus_arm.set_start_state_to_current_state() - # crane_plus_arm.set_goal_state(configuration_name="vertical") - # plan_and_execute( - # crane_plus, - # crane_plus_arm, - # logger, - # single_plan_parameters=arm_plan_request_params, - # ) + # armを現在の位置から"vertical"ポジションに動かす + crane_plus_arm.set_start_state_to_current_state() + crane_plus_arm.set_goal_state(configuration_name="vertical") + plan_and_execute( + crane_plus, + crane_plus_arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) - # # gripperの開閉をデフォルトの間隔に戻す - # robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) - # crane_plus_gripper.set_start_state_to_current_state() - # crane_plus_gripper.set_goal_state(robot_state=robot_state) - # plan_and_execute( - # crane_plus, - # crane_plus_gripper, - # logger, - # single_plan_parameters=gripper_plan_request_params, - # ) + # gripperの開閉をデフォルトの間隔に戻す + robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + crane_plus_gripper.set_start_state_to_current_state() + crane_plus_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + crane_plus_gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) # MoveItPyの終了 crane_plus.shutdown() From b227184b6d1dee6a1f3b8044abe900baa7ccb35a Mon Sep 17 00:00:00 2001 From: mizonon Date: Mon, 21 Oct 2024 11:58:13 +0900 Subject: [PATCH 039/111] =?UTF-8?q?README.md(jazzy=E5=AF=BE=E5=BF=9C?= =?UTF-8?q?=EF=BC=86=E3=83=90=E3=82=A4=E3=83=8A=E3=83=AA=E3=82=A4=E3=83=B3?= =?UTF-8?q?=E3=82=B9=E3=83=88=E3=83=BC=E3=83=AB=E6=96=B9=E6=B3=95=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/README.md b/README.md index 99fbaba5..b30a063f 100644 --- a/README.md +++ b/README.md @@ -21,17 +21,17 @@ ROS 2 package suite of CRANE+ V2. ## Supported ROS 2 distributions - [Foxy](https://github.com/rt-net/crane_plus/tree/foxy-devel) -- Humble - +- [Humble](https://github.com/rt-net/crane_plus/tree/humble-devel) +- Jazzy ## Requirements - CRANE+ V2 - [Product Introduction](https://rt-net.jp/products/cranev2/) - [Web Shop](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1348_1&products_id=3626&language=ja) - 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 @@ -43,14 +43,15 @@ ROS 2 package suite of CRANE+ V2. ### Binary installation ```sh -TBD +$ sudo apt update +$ sudo apt install ros-jazzy-crane-plus ``` ### Source Build ```sh # Setup ROS environment -$ source /opt/ros/humble/setup.bash +$ source /opt/ros/jazzy/setup.bash # Download crane_plus repository $ mkdir -p ~/ros2_ws/src From 88f431342ddc5c9ef3e26ad8a99a1fd9cc19f405 Mon Sep 17 00:00:00 2001 From: mizonon Date: Mon, 21 Oct 2024 12:02:04 +0900 Subject: [PATCH 040/111] =?UTF-8?q?.docker=E3=81=AEREADME.md=E3=81=AEjazzy?= =?UTF-8?q?=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .docker/README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/.docker/README.md b/.docker/README.md index 51fda6ee..c570f387 100644 --- a/.docker/README.md +++ b/.docker/README.md @@ -7,10 +7,10 @@ https://github.com/rt-net/crane_plus/pkgs/container/crane_plus をアップロードしています。 tagにはROSのディストリビューションを指定してください。 -Humbleディストリビューションのイメージをダウンロードする場合は次のコマンドを実行します。 +Jazzyディストリビューションのイメージをダウンロードする場合は次のコマンドを実行します。 ```sh -$ docker pull ghcr.io/rt-net/crane_plus:humble +$ docker pull ghcr.io/rt-net/crane_plus:jazzy ``` ### ノードの起動 @@ -67,9 +67,9 @@ $ rocker --x11 --net=host --privileged \ `./build_source.sh $ROS_DISTRO`を実行してイメージを作成します。 ```sh -# humbleディストリビューションのイメージを作成する +# jazzyディストリビューションのイメージを作成する $ cd crane_plus/.docker -$ ./build_source.sh humble +$ ./build_source.sh jazzy ... -Successfully tagged crane_plus:humble +Successfully tagged crane_plus:jazzy ``` From e9a548f4ad9a13f393fc907622046ed95dc1168c Mon Sep 17 00:00:00 2001 From: mizonon Date: Mon, 21 Oct 2024 17:02:29 +0900 Subject: [PATCH 041/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C=EF=BC=88copyright?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/setup_assistant.launch.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py index e16485a5..3fcdd9d9 100755 --- a/crane_plus_moveit_config/launch/setup_assistant.launch.py +++ b/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -1,3 +1,18 @@ +# Copyright 2020 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. + + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_setup_assistant_launch From bf56ec0646730d113f2ae9aadf3acde893dc0557 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 11:48:42 +0900 Subject: [PATCH 042/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/run_move_group.launch.py | 62 ++++++++++--------- 1 file changed, 33 insertions(+), 29 deletions(-) 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 c51d7ad4..ba9c9b6b 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -18,79 +18,83 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -import yaml from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launch_utils import DeclareBooleanLaunchArg 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.launch_utils import DeclareBooleanLaunchArg +from moveit_configs_utils.launches \ + import generate_static_virtual_joint_tfs_launch def generate_launch_description(): ld = LaunchDescription() declare_loaded_description = DeclareLaunchArgument( - "loaded_description", - default_value="", - description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + 'loaded_description', + default_value='', + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in \ + crane_plus_description.', ) ld.add_action(declare_loaded_description) - ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) - + ld.add_action(DeclareBooleanLaunchArg('debug', default_value=False)) + ld.add_action( DeclareLaunchArgument( - "rviz_config", - default_value=get_package_share_directory("crane_plus_moveit_config") - + "/config/moveit.rviz", - description="Set the path to rviz configuration file.", + 'rviz_config', + default_value=get_package_share_directory( + 'crane_plus_moveit_config' + ) + + '/config/moveit.rviz', + description='Set the path to rviz configuration file.', ) ) - + declare_rviz_config_file = DeclareLaunchArgument( 'rviz_config_file', - default_value=get_package_share_directory('crane_plus_moveit_config') + '/launch/run_move_group.rviz', + default_value=get_package_share_directory('crane_plus_moveit_config') + + '/launch/run_move_group.rviz', description='Set the path to rviz configuration file.' ) - + ld.add_action(declare_rviz_config_file) - + moveit_config = ( - MoveItConfigsBuilder("crane_plus") + MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) .robot_description( file_path=os.path.join( - get_package_share_directory("crane_plus_description"), - "urdf", - "crane_plus.urdf.xacro", + get_package_share_directory('crane_plus_description'), + 'urdf', + 'crane_plus.urdf.xacro', ), mappings={}, ) .robot_description_semantic( - file_path="config/crane_plus.srdf", - mappings={"model": "crane_plus"}, + file_path='config/crane_plus.srdf', + mappings={'model': 'crane_plus'}, ) - .joint_limits(file_path="config/joint_limits.yaml") + .joint_limits(file_path='config/joint_limits.yaml') .trajectory_execution( - file_path="config/controllers.yaml", moveit_manage_controllers=True + file_path='config/controllers.yaml', moveit_manage_controllers=True ) - .planning_pipelines(pipelines=["ompl"]) - .robot_description_kinematics(file_path="config/kinematics.yaml") + .planning_pipelines(pipelines=['ompl']) + .robot_description_kinematics(file_path='config/kinematics.yaml') .to_moveit_configs() ) moveit_config.robot_description = { - "robot_description": LaunchConfiguration("loaded_description") + 'robot_description': LaunchConfiguration('loaded_description') } moveit_config.move_group_capabilities = { - "capabilities": "" + 'capabilities': '' } # Move group From a74f3b74eb078fb62c3392d7e9a88e4790eac0e8 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 12:24:20 +0900 Subject: [PATCH 043/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C(setup=5Fassistant.?= =?UTF-8?q?launch.py)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/launch/setup_assistant.launch.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py index 3fcdd9d9..77eb4d9f 100755 --- a/crane_plus_moveit_config/launch/setup_assistant.launch.py +++ b/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -18,5 +18,7 @@ def generate_launch_description(): - moveit_config = MoveItConfigsBuilder("crane_plus", package_name="crane_plus_moveit_config").to_moveit_configs() + moveit_config = MoveItConfigsBuilder( + 'crane_plus', package_name='crane_plus_moveit_config' + ).to_moveit_configs() return generate_setup_assistant_launch(moveit_config) From a671d5e6e218dd6c394e3d5b42cd367f0adcde20 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 14:33:45 +0900 Subject: [PATCH 044/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C(package.xml?= =?UTF-8?q?=E3=81=AE=E4=B8=8D=E8=A6=81=E3=81=AA=E9=83=A8=E5=88=86=E5=89=8A?= =?UTF-8?q?=E9=99=A4)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/package.xml | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/crane_plus_moveit_config/package.xml b/crane_plus_moveit_config/package.xml index df03651c..f0ee884e 100755 --- a/crane_plus_moveit_config/package.xml +++ b/crane_plus_moveit_config/package.xml @@ -10,13 +10,8 @@ Atsushi Kuwagata Nozomi Mizoguchi - http://moveit.ros.org/ - https://github.com/moveit/moveit2/issues - https://github.com/moveit/moveit2 - ament_cmake - moveit_ros_move_group moveit_kinematics moveit_planners moveit_simple_controller_manager @@ -35,7 +30,7 @@ rviz2 rviz_common rviz_default_plugins - tf2_ros + ament_lint_auto ament_lint_common From 86513fbcfca651861ce1aeb7c32c8f347c792e16 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 14:46:56 +0900 Subject: [PATCH 045/111] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E3=82=92jazzy=E3=81=AB=E5=90=88=E3=82=8F=E3=81=9B=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .docker/build_source.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.docker/build_source.sh b/.docker/build_source.sh index 26ae5e94..b49c45bc 100755 --- a/.docker/build_source.sh +++ b/.docker/build_source.sh @@ -4,7 +4,7 @@ set -e if [ $# -eq 0 ]; then echo "Please set ROS_DISTRO to the argument." - echo "e.g. ./build_source.sh humble" + echo "e.g. ./build_source.sh jazzy" fi ROS_DISTRO=$1 From 2de4b1401a2f15498759ad43e1e0a5c41e096884 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 15:13:07 +0900 Subject: [PATCH 046/111] =?UTF-8?q?ign=20fuel=E3=81=A7gazebo=E3=81=AE?= =?UTF-8?q?=E3=83=A2=E3=83=87=E3=83=AB=E3=82=92=E3=83=80=E3=82=A6=E3=83=B3?= =?UTF-8?q?=E3=83=AD=E3=83=BC=E3=83=89=E3=81=99=E3=82=8B=E8=A8=98=E8=BF=B0?= =?UTF-8?q?=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .docker/source/Dockerfile | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index fa674b61..f5da789d 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -1,4 +1,4 @@ -ARG ROS_DISTRO="humble" +ARG ROS_DISTRO="jazzy" FROM osrf/ros:${ROS_DISTRO}-desktop ENV OVERLAY_WS /root/overlay_ws WORKDIR $OVERLAY_WS/src @@ -31,12 +31,6 @@ RUN mkdir -p /root/.gazebo/models && \ git pull origin master && \ rm -rf .git -# Download ignition model -RUN ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/sun && \ - ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/ground%20plane && \ - ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/wood%20cube%205cm && \ - ign fuel download -v 4 -u https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/table - # Edit entrypoint to source overlay setup file WORKDIR $OVERLAY_WS RUN sed --in-place --expression \ From 8a157cc2e9d82785bb9674a662a68852c3097e3c Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 16:32:57 +0900 Subject: [PATCH 047/111] =?UTF-8?q?ci=E3=81=AEDocker=E3=82=A4=E3=83=A1?= =?UTF-8?q?=E3=83=BC=E3=82=B8=E3=82=92jazzy=E3=81=AB=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/build_docker_image.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build_docker_image.yml b/.github/workflows/build_docker_image.yml index ff083505..0c5f9cd4 100644 --- a/.github/workflows/build_docker_image.yml +++ b/.github/workflows/build_docker_image.yml @@ -7,7 +7,7 @@ on: workflow_dispatch: env: - ROS_DISTRO: humble + ROS_DISTRO: jazzy REGISTRY: ghcr.io jobs: From f1bfe95f9c9c2682d6be8f71d2dca0de23908599 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 22 Oct 2024 16:50:10 +0900 Subject: [PATCH 048/111] =?UTF-8?q?=E4=BB=96=E3=81=AE=E8=A3=BD=E5=93=81?= =?UTF-8?q?=E3=81=A8=E8=A8=AD=E5=AE=9A=E3=82=92=E5=85=B1=E9=80=9A=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/config/ompl_planning.yaml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/crane_plus_moveit_config/config/ompl_planning.yaml b/crane_plus_moveit_config/config/ompl_planning.yaml index 3e0299c2..2c045524 100644 --- a/crane_plus_moveit_config/config/ompl_planning.yaml +++ b/crane_plus_moveit_config/config/ompl_planning.yaml @@ -11,10 +11,14 @@ response_adapters: - default_planning_response_adapters/AddTimeOptimalParameterization - default_planning_response_adapters/ValidateSolution - default_planning_response_adapters/DisplayMotionPath - -start_state_max_bounds_error: 0.1 - 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() From e6866a2fe777eb3d80dbaa397f5d7e27b72d19af Mon Sep 17 00:00:00 2001 From: mizonon Date: Wed, 23 Oct 2024 11:44:30 +0900 Subject: [PATCH 049/111] =?UTF-8?q?crane=5Fplus=5Fdescription/package.xml?= =?UTF-8?q?=E3=81=AEauthor=E3=81=AB=E6=BA=9D=E5=8F=A3=E3=82=92=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_description/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_plus_description/package.xml b/crane_plus_description/package.xml index 1e5ff7ec..2ac40181 100755 --- a/crane_plus_description/package.xml +++ b/crane_plus_description/package.xml @@ -9,6 +9,7 @@ Shota Aoki Atsushi Kuwagata + Nozomi Mizoguchi ament_cmake ament_cmake_python From f2e153ac6d3904a7692bf571afc184501deed07e Mon Sep 17 00:00:00 2001 From: mizonon Date: Wed, 23 Oct 2024 17:19:52 +0900 Subject: [PATCH 050/111] =?UTF-8?q?ompl=5Fplanning.yaml=E3=81=AB=E6=8A=9C?= =?UTF-8?q?=E3=81=91=E3=81=A6=E3=81=84=E3=81=9F=E8=A8=98=E8=BF=B0=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/config/ompl_planning.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crane_plus_moveit_config/config/ompl_planning.yaml b/crane_plus_moveit_config/config/ompl_planning.yaml index 2c045524..0d88b7fa 100644 --- a/crane_plus_moveit_config/config/ompl_planning.yaml +++ b/crane_plus_moveit_config/config/ompl_planning.yaml @@ -145,6 +145,7 @@ planner_configs: arm: planner_configs: + - APSConfigDefault - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault @@ -171,6 +172,7 @@ arm: - TrajOptDefault gripper: planner_configs: + - APSConfigDefault - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault From b895f0b8ed134ee8c59a38efc6ffb40e31263f1d Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 25 Oct 2024 16:30:21 +0900 Subject: [PATCH 051/111] =?UTF-8?q?joint=5Fvalues.py=E3=81=ABmax=5Facceler?= =?UTF-8?q?aration=5Fscaling=5Ffactor=E3=81=A8max=5Fvelocity=5Fscaling=5Ff?= =?UTF-8?q?actor=E3=81=AE=E8=A8=AD=E5=AE=9A=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/joint_values.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 6e1cd289..687b56d5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -50,7 +50,10 @@ def main(args=None): crane_plus, "ompl_rrtc", ) - + + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + # armの関節のjoint1〜4を順番に45[deg]ずつ動かす joint_names = ["crane_plus_joint1", "crane_plus_joint2", "crane_plus_joint3", "crane_plus_joint4"] target_joint_value = math.radians(45) From 8eaf1e86dc96fc51a0945cd6c0039dc5ed1c3675 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 25 Oct 2024 18:49:02 +0900 Subject: [PATCH 052/111] =?UTF-8?q?color=5Fdetection.py=E3=81=AEsubscripti?= =?UTF-8?q?on=E3=81=AEcallback=E9=96=A2=E6=95=B0=E5=90=8D=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/color_detection.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index 17ea59cf..69783445 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -28,10 +28,10 @@ class ImageSubscriber(Node): def __init__(self): super().__init__("color_detection") self.image_subscription = self.create_subscription( - Image, "image_raw", self.listener_callback, 10 + Image, "image_raw", self.image_callback, 10 ) self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.listener_callback, 10 + CameraInfo, "camera_info", self.camera_info_callback, 10 ) self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) self.tf_broadcaster = tf2_ros.TransformBroadcaster() From 5c6104ddbdf457bc596f0b47aac4182c4de8c41d Mon Sep 17 00:00:00 2001 From: mizonon Date: Wed, 30 Oct 2024 16:51:03 +0900 Subject: [PATCH 053/111] =?UTF-8?q?crane=5Fplus=5Fexamples=5Fpy=E7=94=A8?= =?UTF-8?q?=E3=81=AEREADME=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 122 ---------- crane_plus_examples_py/README.md | 230 ++++++++++++++++++ .../crane_plus_examples_py/aruco_detection.py | 4 +- 3 files changed, 232 insertions(+), 124 deletions(-) delete mode 100644 README.md create mode 100644 crane_plus_examples_py/README.md diff --git a/README.md b/README.md deleted file mode 100644 index 99fbaba5..00000000 --- a/README.md +++ /dev/null @@ -1,122 +0,0 @@ -# crane_plus - -[![industrial_ci](https://github.com/rt-net/crane_plus/workflows/industrial_ci/badge.svg?branch=master)](https://github.com/rt-net/crane_plus/actions?query=workflow%3Aindustrial_ci+branch%3Amaster) - -ROS 2 package suite of CRANE+ V2. - - - -## Table of Contents - -- [Supported ROS 2 distributions](#supported-ROS-2-distributions) -- [Requirements](#requirements) -- [Installation](#installation) - - [Binary installation](#binary-installation) - - [Source Build](#source-build) -- [Quick Start](#quick-start) -- [Packages](#packages) -- [License](#license) -- [Disclaimer](#disclaimer) - -## Supported ROS 2 distributions - -- [Foxy](https://github.com/rt-net/crane_plus/tree/foxy-devel) -- Humble - -## Requirements - -- CRANE+ V2 - - [Product Introduction](https://rt-net.jp/products/cranev2/) - - [Web Shop](https://www.rt-shop.jp/index.php?main_page=product_info&cPath=1348_1&products_id=3626&language=ja) -- Linux OS - - Ubuntu 22.04 -- ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) - -## Installation - -### Docker images - -ビルド済みのパッケージ含むDocker imageを用意してます。 -詳細は[.docker/README.md](./.docker/README.md)を参照してください。 - -### Binary installation - -```sh -TBD -``` - -### Source Build - -```sh -# Setup ROS environment -$ source /opt/ros/humble/setup.bash - -# Download crane_plus repository -$ mkdir -p ~/ros2_ws/src -$ cd ~/ros2_ws/src -$ git clone https://github.com/rt-net/crane_plus.git - -# Install dependencies -$ rosdep install -r -y -i --from-paths . - -# Build & Install -$ cd ~/ros2_ws -$ colcon build --symlink-install -$ source ~/ros2_ws/install/setup.bash -``` - -## Quick Start - -```sh -# Connect CRANE+ V2 to PC, then -$ source ~/ros2_ws/install/setup.bash -$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 - -# Terminal 2 -$ source ~/ros2_ws/install/setup.bash -$ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' - -# Press [Ctrl-c] to terminate. -``` - - - -詳細は[crane_plus_examples](./crane_plus_examples/README.md) -を参照してください。 - -## Packages - -- crane_plus_control - - [README](./crane_plus_control/README.md) - - CRANE+ V2を制御するパッケージです - - USB通信ポートの設定方法をREAMDEに記載してます -- crane_plus_description - - [README](./crane_plus_description/README.md) - - CRANE+ V2のモデルデータ（xacro）を定義するパッケージです -- crane_plus_examples - - [README](./crane_plus_examples/README.md) - - CRANE+ V2のサンプルコード集です -- crane_plus_gazebo - - [README](./crane_plus_gazebo/README.md) - - CRANE+ V2のGazeboシミュレーションパッケージです -- crane_plus_moveit_config - - [README](./crane_plus_moveit_config/README.md) - - CRANE+ V2の`moveit2`設定ファイルです - -## License - -このリポジトリはApache 2.0ライセンスの元、公開されています。 -ライセンスについては[LICENSE](./LICENSE)を参照ください。 - -サーボモータのAX-12Aに関するCADモデルの使用については、ROBOTIS社より使用許諾を受けています。 -CRANE+ V2に使用されているROBOTIS社の部品類にかかる著作権、商標権、その他の知的財産権は、ROBOTIS社に帰属します。 - -We have obtained permission from ROBOTIS Co., Ltd. to use CAD models relating to servo motors AX-12A. The proprietary rights relating to any components or parts manufactured by ROBOTIS and used in this product, including but not limited to copyrights, trademarks, and other intellectual property rights, shall remain vested in ROBOTIS. - -## Disclaimer - -本ソフトウェアはApache 2.0ライセンスで、「AS IS」（現状有姿のまま）で提供しています。本ソフトウェアに関する無償サポートはありません。 - -当該製品および当ソフトウェアの使用中に生じたいかなる損害も株式会社アールティでは一切の責任を負いかねます。 ユーザー自身で作成されたプログラムに適切な制限動作が備わっていない場合、本体の損傷や、本体が周囲や作業者に接触、あるいは衝突し、思わぬ重大事故が発生する危険があります。 ユーザーの責任において十分に安全に注意した上でご使用下さい。 - diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md new file mode 100644 index 00000000..000e3676 --- /dev/null +++ b/crane_plus_examples_py/README.md @@ -0,0 +1,230 @@ +# crane_plus_examples_py + +このパッケージはCRANE+ V2 ROS 2パッケージのPythonによるサンプルコード集です。 + +## 準備（実機を使う場合） + +![crane_plus](https://rt-net.github.io/images/crane-plus/CRANEV2-500x500.png) + +### 1. CRANE+ V2本体をPCに接続する + +CRANE+ V2本体をPCに接続します。 +接続方法は製品マニュアルを参照してください。 + +**※CRANE+ V2本体が接触しないように、十分なスペースを確保してください。** + +### 2. USB通信ポートの接続を確認する + +USB通信ポートの設定については`crane_plus_control`の +[README](../crane_plus_control/README.md) +を参照してください。 + +**正しく設定できていない場合、CRANE+ V2が動作しない、振動する、などの不安定な動きになるので注意してください** + +### 3. move_groupとcontrollerを起動する + +#### 標準のCRANE+ V2を使用する場合 + +次のコマンドでmove_group (`crane_plus_moveit_config`)と +controller (`crane_plus_control`)を起動します。 + +```sh +$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 +``` + +#### Webカメラ搭載モデルを使用する場合 + +Webカメラ搭載モデルの場合は、次のコマンドを実行してください。 +```video_device```は使用するWebカメラを指定してください。 + +```sh +$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 use_camera:=true video_device:=/dev/video0 +``` + +## 準備（Gazeboを使う場合） +======= +![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png) + +### 1. move_groupとGazeboを起動する + +次のコマンドでmove_group (`crane_plus_moveit_config`)とGazeboを起動します。 + +```sh +$ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py +``` + +## Pythonのサンプルプログラムを実行する + +準備ができたらPythonによるサンプルプログラムを実行します。 +例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +``` + +終了するときは`Ctrl+c`を入力します。 + +## Gazeboでサンプルプログラムを実行する場合 + +Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true +``` + +## Examples + +`demo.launch.py`を実行している状態で各サンプルを実行できます。 + +- [gripper_control](#gripper_control) +- [pose_groupstate](#pose_groupstate) +- [joint_values](#joint_values) +- [pick_and_place](#pick_and_place) + +実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py -s +Arguments (pass arguments as ':='): + + 'example': + Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place] + (default: 'gripper_control') +``` + +--- + +### gripper_control + +グリッパを開閉させるコード例です。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +``` + + + +[back to example list](#examples) + +--- + +### pose_groupstate + +group_stateを使うコード例です。 + +SRDFファイル[crane_plus_moveit_config/config/crane_plus.srdf](../crane_plus_moveit_config/config/crane_plus.srdf) +に記載されている`home`と`vertical`の姿勢に移行します。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate' +``` + + + +[back to example list](#examples) + +--- + +### joint_values + +アームのジョイント角度を１つずつ変更するコード例です。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='joint_values' +``` + + + +[back to example list](#examples) + +--- + +### pick_and_place + +モノを掴む・持ち上げる・運ぶ・置くコード例です。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place' +``` + + + +[back to example list](#examples) + +--- + +## Camera Examples + +Webカメラ搭載モデルのカメラを使用したサンプルコードです。 + +[「Webカメラ搭載モデルを使用する場合」](#Webカメラ搭載モデルを使用する場合)の手順に従って、 +`demo.launch`を実行している状態で、 +各サンプルを実行できます。 + +- [aruco\_detection](#aruco_detection) +- [color\_detection](#color_detection) + +実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 + +```sh +$ ros2 launch crane_plus_examples_py camera_example.launch.py -s +Arguments (pass arguments as ':='): + + 'example': + Set an example executable name: [color_detection] + (default: 'color_detection') +``` + +--- + +### aruco_detection + +モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。 +マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷して、一辺50mmの立方体に取り付けて使用します。 + +検出されたマーカの位置姿勢はtfのフレームとして配信されます。 +tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。 +掴む対象は`target_0`に設定されています。 +マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。 + +次のコマンドを実行します。 + +```bash +ros2 launch crane_plus_examples_py camera_example.launch.py example:='aruco_detection' +``` + +#### Videos +[![crane_plus_aruco_detection_demo](https://rt-net.github.io/images/crane-plus/aruco_detection.gif)](https://youtu.be/m9dus6LCocc) + +[back to example list](#examples) + +--- + +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +デフォルトでは赤い物体の位置をtfのフレームとして配信します。 +tfの`frame_id`は`target_0`です。 +色検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py camera_example.launch.py example:='color_detection' +``` + +#### Videos +[![crane_plus_color_detection_demo](https://rt-net.github.io/images/crane-plus/color_detection.gif)](https://youtu.be/Kn0eWA7sALY) + +[back to example list](#examples) + +--- diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index c3acd628..a7fac85f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -33,10 +33,10 @@ class ImageSubscriber(Node): def __init__(self): super().__init__("aruco_detection") self.image_subscription = self.create_subscription( - Image, "image_raw", self.listener_callback, 10 + Image, "image_raw", self.image_callback, 10 ) self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.listener_callback, 10 + CameraInfo, "camera_info", self.camera_info_callback, 10 ) self.tf_broadcaster = tf2_ros.TransformBroadcaster() From 4e0a275f500d6502ae077eea25f92bc4981a9321 Mon Sep 17 00:00:00 2001 From: mizonon Date: Thu, 31 Oct 2024 12:32:20 +0900 Subject: [PATCH 054/111] =?UTF-8?q?CI=E3=81=AEnumpy-quaternion=E3=81=AE?= =?UTF-8?q?=E3=82=A4=E3=83=B3=E3=82=B9=E3=83=88=E3=83=BC=E3=83=AB=E3=82=A8?= =?UTF-8?q?=E3=83=A9=E3=83=BC=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 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index d809d96e..f7fc88fc 100755 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -21,5 +21,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v2 + - name: Set environment variable for pip + run: echo "PIP_BREAK_SYSTEM_PACKAGES=1" >> $GITHUB_ENV - uses: "ros-industrial/industrial_ci@master" env: ${{ matrix.env }} From 057338ce7ebc3b2b855883a193888124597d501f Mon Sep 17 00:00:00 2001 From: mizonon Date: Thu, 31 Oct 2024 15:09:00 +0900 Subject: [PATCH 055/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples/launch/demo.launch.py | 2 +- .../crane_plus_examples_py/aruco_detection.py | 56 ++++---- .../crane_plus_examples_py/color_detection.py | 51 ++++---- .../crane_plus_examples_py/gripper_control.py | 30 ++--- .../crane_plus_examples_py/joint_values.py | 36 +++--- .../pick_and_place_tf.py | 122 +++++++++--------- .../crane_plus_examples_py/utils.py | 28 ++-- crane_plus_examples_py/setup.py | 9 +- 8 files changed, 159 insertions(+), 175 deletions(-) diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 17cd579c..12a7dbeb 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): default_value='/dev/ttyUSB0', description='Set port name.' ) - + declare_use_camera = DeclareLaunchArgument( 'use_camera', default_value='false', diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index a7fac85f..b17a1d9f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -1,45 +1,40 @@ # Copyright 2020 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 rclpy -from rclpy.node import Node - -import numpy as np - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo, Image import cv2 from cv2 import aruco from cv_bridge import CvBridge -import tf2_ros - from crane_plus_examples_py.utils import rotation_matrix_to_quaternion +from geometry_msgs.msg import TransformStamped +import numpy as np +import rclpy +from rclpy.node import Node +import tf2_ros +from sensor_msgs.msg import CameraInfo, Image class ImageSubscriber(Node): def __init__(self): - super().__init__("aruco_detection") + super().__init__('aruco_detection') self.image_subscription = self.create_subscription( - Image, "image_raw", self.image_callback, 10 + Image, 'image_raw', self.image_callback, 10 ) self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.camera_info_callback, 10 + CameraInfo, 'camera_info', self.camera_info_callback, 10 ) self.tf_broadcaster = tf2_ros.TransformBroadcaster() - + self.camera_info = None self.bridge = CvBridge() @@ -47,31 +42,31 @@ def image_callback(self, msg): # 画像データをROSのメッセージからOpenCVの配列に変換 cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) - + if self.camera_info: # ArUcoマーカのデータセットを読み込む # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) - + # マーカーID ids = [] - + # 画像座標系上のマーカ頂点位置 corners = [] - + # マーカの検出 corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) # マーカの検出数 n_markers = len(ids) - + # カメラパラメータ CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) - + # マーカ一辺の長さ 0.04 [m] MARKER_LENGTH = 0.04 - + # マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 if n_markers > 0: for i in range(n_markers): @@ -81,7 +76,7 @@ def image_callback(self, msg): # tfの配信 t = TransformStamped() t.header = msg.header - t.child_frame_id = "target_" + str(ids[i][0]) + t.child_frame_id = 'target_' + str(ids[i][0]) t.transform.translation.x = tvecs[i][0][0] t.transform.translation.y = tvecs[i][0][1] t.transform.translation.z = tvecs[i][0][2] @@ -94,20 +89,17 @@ def image_callback(self, msg): t.transform.rotation.z = q.z t.transform.rotation.w = q.w self.tf_broadcaster.sendTransform(t) - + def camera_info_callback(self, msg): self.camera_info = msg - - + + def main(args=None): rclpy.init(args=args) image_subscriber = ImageSubscriber() rclpy.spin(image_subscriber) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) image_subscriber.destroy_node() rclpy.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index 69783445..ae31d84f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -1,27 +1,25 @@ # Copyright 2020 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 rclpy -from rclpy.node import Node - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo, Image -from image_geometry import PinholeCameraModel import cv2 from cv_bridge import CvBridge +from geometry_msgs.msg import TransformStamped +from image_geometry import PinholeCameraModel +import rclpy +from rclpy.node import Node import tf2_ros +from sensor_msgs.msg import CameraInfo, Image class ImageSubscriber(Node): @@ -50,24 +48,24 @@ def image_callback(self, msg): HIGH_S = 255 LOW_V = 50 HIGH_V = 255 - + # ウェブカメラの画像を受け取る cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) - + # 画像の二値化 img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) # マスク画像の合成 img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) - + # ノイズ除去の処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) - + # 穴埋めの処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) @@ -77,26 +75,26 @@ def image_callback(self, msg): d_m01 = moment['m01'] d_m10 = moment['m10'] d_area = moment['m00'] - + # 検出した領域のピクセル数が10000より大きい場合 if d_area < 10000: # カメラモデル作成 camera_model = PinholeCameraModel() - + # カメラのパラメータを設定 camera_model.fromCameraInfo(self.camera_info) - + # 画像座標系における把持対象物の位置（2D） pixel_x = d_m10 / d_area pixel_y = d_m01 / d_area point = (pixel_x, pixel_y) - + # 補正後の画像座標系における把持対象物の位置を取得（2D） rect_point = camera_model.rectifyImage(point) - + # カメラ座標系から見た把持対象物の方向（Ray）を取得する ray = camera_model.projectPixelTo3dRay(rect_point) - + # カメラの高さを0.44[m]として把持対象物の位置を計算 CAMERA_HEIGHT = 0.46 object_position = { @@ -104,7 +102,7 @@ def image_callback(self, msg): 'y': ray.y * CAMERA_HEIGHT, 'z': ray.z * CAMERA_HEIGHT, } - + # 把持対象物の位置をTFに配信 t = TransformStamped() t.header = msg.header @@ -113,14 +111,14 @@ def image_callback(self, msg): t.transform.translation.y = object_position['y'] t.transform.translation.z = object_position['z'] self.tf_broadcaster.sendTransform(t) - + # 閾値による二値化画像を配信 img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8") self.image_thresholded_publisher.publish(img_thresholded_msg) - + def camera_info_callback(self, msg): self.camera_info = msg - + def main(args=None): rclpy.init(args=args) @@ -128,12 +126,9 @@ def main(args=None): image_subscriber = ImageSubscriber() rclpy.spin(image_subscriber) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) image_subscriber.destroy_node() rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index d1b351bf..bb5f8b90 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -12,11 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from crane_plus_examples_py.utils import plan_and_execute import math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger # moveit python library from moveit.core.robot_state import RobotState @@ -24,20 +21,21 @@ MoveItPy, PlanRequestParameters, ) - -from crane_plus_examples_py.utils import plan_and_execute +import rclpy +from rclpy.logging import get_logger def main(args=None): # ros2の初期化 rclpy.init(args=args) + # ロガー生成 - logger = get_logger("gripper_control") + logger = get_logger('gripper_control') # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_gripper = crane_plus.get_planning_component("gripper") - logger.info("MoveItPy instance created") + crane_plus = MoveItPy(node_name='moveit_py') + crane_plus_gripper = crane_plus.get_planning_component('gripper') + logger.info('MoveItPy instance created') # instantiate a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() @@ -46,7 +44,7 @@ def main(args=None): # planningのパラメータ設定 plan_request_params = PlanRequestParameters( crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) # 速度＆加速度のスケーリングファクタを設定 @@ -54,7 +52,7 @@ def main(args=None): plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # グリッパーを+30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(30)]) + robot_state.set_joint_group_positions('gripper', [math.radians(30)]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -63,8 +61,9 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) + # グリッパーを-30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(-30)]) + robot_state.set_joint_group_positions('gripper', [math.radians(-30)]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -73,8 +72,9 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) + # グリッパーを0[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [0.0]) + robot_state.set_joint_group_positions('gripper', [0.0]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -83,7 +83,7 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) - + # MoveItPyの終了 crane_plus.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 687b56d5..35bdc02f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -12,13 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from crane_plus_examples_py.utils import plan_and_execute import math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger - # moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( @@ -26,8 +22,8 @@ PlanRequestParameters, ) from moveit.core.kinematic_constraints import construct_joint_constraint - -from crane_plus_examples_py.utils import plan_and_execute +import rclpy +from rclpy.logging import get_logger def main(args=None): @@ -35,37 +31,41 @@ def main(args=None): rclpy.init(args=args) # ロガー生成 - logger = get_logger("joint_values") + logger = get_logger('joint_values') # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - logger.info("MoveItPy instance created") - + crane_plus = MoveItPy(node_name='moveit_py') + crane_plus_arm = crane_plus.get_planning_component('arm') + logger.info('MoveItPy instance created') + # instantiate a a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() robot_state = RobotState(robot_model) plan_request_params = PlanRequestParameters( crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # armの関節のjoint1〜4を順番に45[deg]ずつ動かす - joint_names = ["crane_plus_joint1", "crane_plus_joint2", "crane_plus_joint3", "crane_plus_joint4"] + joint_names = ['crane_plus_joint1', 'crane_plus_joint2', + 'crane_plus_joint3', 'crane_plus_joint4'] target_joint_value = math.radians(45) for joint_name in joint_names: joint_values = {joint_name: target_joint_value} robot_state.joint_positions = joint_values joint_constraint = construct_joint_constraint( robot_state=robot_state, - joint_model_group=crane_plus.get_robot_model().get_joint_model_group("arm"), + joint_model_group=crane_plus + .get_robot_model().get_joint_model_group('arm'), ) crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + crane_plus_arm.set_goal_state( + motion_plan_constraints=[joint_constraint] + ) plan_and_execute( crane_plus, crane_plus_arm, @@ -73,9 +73,9 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # 現在の位置から"vertical"ポジションに動かす + # 現在の位置から'vertical'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, crane_plus_arm, diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index d0ea07d8..779e463e 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -1,59 +1,55 @@ # Copyright 2020 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 math -import numpy as np +from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion import datetime - -# import angles from geometry_msgs.msg import Pose -from moveit_msgs.msg import Constraints, JointConstraint -import rclpy -from rclpy.time import Time -from rclpy.node import Node -from rclpy.logging import get_logger -import tf2_ros -from tf2_ros import TransformStamped, TransformListener -from tf2_ros.buffer import Buffer - +import math +import numpy as np # moveit python library from moveit.core.robot_state import RobotState +from moveit_msgs.msg import Constraints, JointConstraint from moveit.planning import ( MoveItPy, PlanRequestParameters, ) - -from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion +import rclpy +from rclpy.logging import get_logger +from rclpy.node import Node +from rclpy.time import Time +import tf2_ros +from tf2_ros import TransformStamped, TransformListener +from tf2_ros.buffer import Buffer class PickAndPlaceTf(Node): def __init__(self, crane_plus): - super().__init__("pick_and_place_tf_node") - self.logger = get_logger("pick_and_place_tf") + super().__init__('pick_and_place_tf_node') + self.logger = get_logger('pick_and_place_tf') self.tf_past = TransformStamped() self.crane_plus = crane_plus - self.crane_plus_arm = crane_plus.get_planning_component("arm") - self.crane_plus_gripper = crane_plus.get_planning_component("gripper") + self.crane_plus_arm = crane_plus.get_planning_component('arm') + self.crane_plus_gripper = crane_plus.get_planning_component('gripper') # instantiate a RobotState instance using the current robot model self.robot_model = crane_plus.get_robot_model() self.robot_state = RobotState(self.robot_model) - + # planningのパラメータ設定 # armのパラメータ設定用 self.arm_plan_request_params = PlanRequestParameters( self.crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 @@ -61,14 +57,14 @@ def __init__(self, crane_plus): # gripperのパラメータ設定用 self.gripper_plan_request_params = PlanRequestParameters( self.crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # SRDFに定義されている"home"の姿勢にする + # SRDFに定義されている'home'の姿勢にする self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state(configuration_name="home") + self.crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( self.crane_plus, self.crane_plus_arm, @@ -76,45 +72,47 @@ def __init__(self, crane_plus): self.logger, single_plan_parameters=self.arm_plan_request_params, ) - + # 可動範囲を制限する constraints = Constraints() - constraints.name = "arm_constraints" - + constraints.name = 'arm_constraints' + jointConstraint = JointConstraint() - jointConstraint.joint_name = "crane_plus_joint1" + jointConstraint.joint_name = 'crane_plus_joint1' jointConstraint.position = 0.0 jointConstraint.tolerance_above = math.radians(100) jointConstraint.tolerance_below = math.radians(100) jointConstraint.weight = 1.0 constraints.joint_constraints.append(jointConstraint) - - jointConstraint.joint_name = "crane_plus_joint3" + + jointConstraint.joint_name = 'crane_plus_joint3' jointConstraint.position = 0.0 jointConstraint.tolerance_above = math.radians(0) jointConstraint.tolerance_below = math.radians(180) jointConstraint.weight = 1.0 constraints.joint_constraints.append(jointConstraint) - + self.crane_plus_arm.set_path_constraints(constraints) - + # 待機姿勢 self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) - + # tf self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) - + # Call on_timer function every second self.timer = self.create_timer(0.5, self.on_timer) - + def on_timer(self): # target_0のtf位置姿勢を取得 try: tf_msg = self.tf_buffer.lookup_transform( 'base_link', 'target_0', Time()) except tf2_ros.LookupException as ex: - self.get_logger().info(f'Could not transform base_link to target: {ex}') + self.get_logger().info( + f'Could not transform base_link to target: {ex}' + ) now = Time() FILTERING_TIME = datetime.timedelta(seconds=2) @@ -129,7 +127,7 @@ def on_timer(self): TF_STOP_TIME = now.nanoseconds - self.tf_past.header.stamp.nanosec else: TF_STOP_TIME = now.nanoseconds - + # 現在時刻から2秒以内に受け取ったtfを使用 if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: tf_diff = np.sqrt((self.tf_past.transform.translation.x @@ -151,53 +149,53 @@ def _picking(self, tf_msg): GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - + # 何かを掴んでいた時のためにハンドを開く self._control_gripper(GRIPPER_OPEN) - + # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 x = tf_msg.transform.translation.x y = tf_msg.transform.translation.y theta_rad = math.atan2(y, x) theta_deg = math.degrees(theta_rad) - + # 把持対象物に正対する self._control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg) - + # 掴みに行く GRIPPER_OFFSET = 0.13 gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) - if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, + if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, 0.05, 0, 90, theta_deg): # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) return - + # ハンドを閉じる self._control_gripper(GRIPPER_CLOSE) - + # 移動する self._control_arm(0.0, 0.0, 0.17, 0, 90, 0) - + # 下ろす self._control_arm(0.0, -0.15, 0.06, 0, 90, -90) - + # ハンドを開く self._control_gripper(GRIPPER_OPEN) - + # 少しだけハンドを持ち上げる self._control_arm(0.0, -0.15, 0.10, 0, 90, -90) - + # 待機姿勢に戻る self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) - + # ハンドを閉じる self._control_gripper(GRIPPER_DEFAULT) - + # グリッパ制御 def _control_gripper(self, angle): - self.robot_state.set_joint_group_positions("gripper", [angle]) + self.robot_state.set_joint_group_positions('gripper', [angle]) self.crane_plus_gripper.set_start_state_to_current_state() self.crane_plus_gripper.set_goal_state(robot_state=self.robot_state) plan_and_execute( @@ -207,7 +205,7 @@ def _control_gripper(self, angle): self.logger, single_plan_parameters=self.gripper_plan_request_params, ) - + # アーム制御 def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose = Pose() @@ -222,7 +220,7 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose.orientation.w = q[3] self.crane_plus_arm.set_start_state_to_current_state() self.crane_plus_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link="crane_plus_link4" + pose_stamped_msg=target_pose, pose_link='crane_plus_link4' ) result = plan_and_execute( self.crane_plus, @@ -232,25 +230,25 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): single_plan_parameters=self.arm_plan_request_params, ) return result - - + + def main(args=None): # ros2の初期化 rclpy.init(args=args) # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") + crane_plus = MoveItPy(node_name='moveit_py') # node生成 pick_and_place_tf_node = PickAndPlaceTf(crane_plus) rclpy.spin(pick_and_place_tf_node) - + # MoveItPyの終了 crane_plus.shutdown() - + # rclpyの終了 rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 56709c9a..3b48f781 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,23 +1,21 @@ # Copyright 2020 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 time import numpy as np -import quaternion - -# generic ros libraries from rclpy.logging import get_logger +import time +import quaternion # plan and execute @@ -31,8 +29,8 @@ def plan_and_execute( ): """Helper function to plan and execute a motion.""" # plan to goal - logger = get_logger("plan_and_execute") - logger.info("Planning trajectory") + logger = get_logger('plan_and_execute') + logger.info('Planning trajectory') if multi_plan_parameters is not None: plan_result = planning_component.plan( multi_plan_parameters=multi_plan_parameters @@ -50,16 +48,16 @@ def plan_and_execute( # execute the plan result = None if plan_result: - logger.info("Executing plan") + logger.info('Executing plan') robot_trajectory = plan_result.trajectory result = robot.execute(robot_trajectory, controllers=[]) else: - logger.error("Planning failed") + logger.error('Planning failed') result = False time.sleep(sleep_time) return result - - + + # euler --> quaternion def euler_to_quaternion(roll, pitch, yaw): cy = np.cos(yaw * 0.5) @@ -80,6 +78,6 @@ def euler_to_quaternion(roll, pitch, yaw): # rotation matrix --> quaternion def rotation_matrix_to_quaternion(rotation_matrix): # numpy-quaternionを使用して回転行列からクォータニオンを計算 - #3x3の回転行列をnumpy.quaternionに変換する + # 3x3の回転行列をnumpy.quaternionに変換する q = quaternion.from_rotation_matrix(rotation_matrix) - return q \ No newline at end of file + return q diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index e6a0e1bf..fcb6446b 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -1,20 +1,21 @@ -import os from glob import glob +import os from setuptools import find_packages, setup package_name = 'crane_plus_examples_py' setup( name=package_name, - # version='0.0.0', version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]'))), - (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*'))), ], install_requires=['setuptools'], zip_safe=True, From 8ab38228fce93ae9561d41201e758f8621eceed1 Mon Sep 17 00:00:00 2001 From: mizonon Date: Thu, 31 Oct 2024 15:48:40 +0900 Subject: [PATCH 056/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 4 +- .../crane_plus_examples_py/color_detection.py | 36 ++++--- .../crane_plus_examples_py/gripper_control.py | 1 + .../crane_plus_examples_py/joint_values.py | 3 +- .../crane_plus_examples_py/pick_and_place.py | 94 ++++++++++--------- .../pick_and_place_tf.py | 21 ++++- .../crane_plus_examples_py/pose_groupstate.py | 30 +++--- 7 files changed, 109 insertions(+), 80 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index b17a1d9f..d886cdd2 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -12,16 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. +from crane_plus_examples_py.utils import rotation_matrix_to_quaternion import cv2 from cv2 import aruco from cv_bridge import CvBridge -from crane_plus_examples_py.utils import rotation_matrix_to_quaternion from geometry_msgs.msg import TransformStamped import numpy as np import rclpy from rclpy.node import Node -import tf2_ros from sensor_msgs.msg import CameraInfo, Image +import tf2_ros class ImageSubscriber(Node): diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index ae31d84f..dd725ac2 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -24,14 +24,16 @@ class ImageSubscriber(Node): def __init__(self): - super().__init__("color_detection") + super().__init__('color_detection') self.image_subscription = self.create_subscription( - Image, "image_raw", self.image_callback, 10 + Image, 'image_raw', self.image_callback, 10 ) self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.camera_info_callback, 10 + CameraInfo, 'camera_info', self.camera_info_callback, 10 ) - self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) + self.image_thresholded_publisher = self.create_publisher( + Image, 'image_thresholded', 10 + ) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None self.bridge = CvBridge() @@ -50,25 +52,35 @@ def image_callback(self, msg): HIGH_V = 255 # ウェブカメラの画像を受け取る - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = self.bridge.imgmsg_to_cv2( + msg, desired_encoding=msg.encoding + ) # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) # 画像の二値化 - img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) - img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) + img_mask_1 = cv2.inRange( + cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V) + ) + img_mask_2 = cv2.inRange( + cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V) + ) # マスク画像の合成 img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) # ノイズ除去の処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + img_thresholded = cv2.morphologyEx( + img_thresholded, cv2.MORPH_OPEN, kernel + ) # 穴埋めの処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + img_thresholded = cv2.morphologyEx( + img_thresholded, cv2.MORPH_CLOSE, kernel + ) # 画像の検出領域におけるモーメントを計算 moment = cv2.moments(img_thresholded) @@ -106,14 +118,16 @@ def image_callback(self, msg): # 把持対象物の位置をTFに配信 t = TransformStamped() t.header = msg.header - t.child_frame_id = "target_0" + t.child_frame_id = 'target_0' t.transform.translation.x = object_position['x'] t.transform.translation.y = object_position['y'] t.transform.translation.z = object_position['z'] self.tf_broadcaster.sendTransform(t) # 閾値による二値化画像を配信 - img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8") + img_thresholded_msg = self.bridge.cv2_to_imgmsg( + img_thresholded, encoding='mono8' + ) self.image_thresholded_publisher.publish(img_thresholded_msg) def camera_info_callback(self, msg): diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index bb5f8b90..7a8e42c3 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -13,6 +13,7 @@ # limitations under the License. from crane_plus_examples_py.utils import plan_and_execute + import math # moveit python library diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 35bdc02f..f8e06f2e 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -12,9 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import plan_and_execute import math +from crane_plus_examples_py.utils import plan_and_execute + # moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 6cbd16ae..7ab7e49d 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -12,13 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -import math - -import rclpy +from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion from geometry_msgs.msg import PoseStamped - -# generic ros libraries -from rclpy.logging import get_logger +import math # moveit python library from moveit.core.robot_state import RobotState @@ -26,8 +22,8 @@ MoveItPy, PlanRequestParameters, ) - -from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion +import rclpy +from rclpy.logging import get_logger def main(args=None): @@ -35,13 +31,13 @@ def main(args=None): rclpy.init(args=args) # ロガー生成 - logger = get_logger("pick_and_place") + logger = get_logger('pick_and_place') # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - crane_plus_gripper = crane_plus.get_planning_component("gripper") - logger.info("MoveItPy instance created") + crane_plus = MoveItPy(node_name='moveit_py') + crane_plus_arm = crane_plus.get_planning_component('arm') + crane_plus_gripper = crane_plus.get_planning_component('gripper') + logger.info('MoveItPy instance created') # instantiate a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() @@ -51,27 +47,35 @@ def main(args=None): # armのパラメータ設定用 arm_plan_request_params = PlanRequestParameters( crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 + + # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # gripperのパラメータ設定用 gripper_plan_request_params = PlanRequestParameters( crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) - gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 + + # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # gripperの開閉角度 GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - # armを現在の位置から"vertical"ポジションに動かす + # armを現在の位置から'vertical'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, crane_plus_arm, @@ -80,7 +84,7 @@ def main(args=None): ) # gripperを0度の位置に動かす - robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -89,11 +93,11 @@ def main(args=None): logger, single_plan_parameters=gripper_plan_request_params, ) - + # ----- Picking Preparation ----- - # armを現在の位置から"home"ポジションに動かす + # armを現在の位置から'home'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") + crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, crane_plus_arm, @@ -102,7 +106,7 @@ def main(args=None): ) # なにか掴んでいたときのためにgripperを開く - robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -114,8 +118,8 @@ def main(args=None): # armが掴みに行く位置を指定して動かす pose_goal = PoseStamped() - pose_goal.header.frame_id = "crane_plus_base" - + pose_goal.header.frame_id = 'crane_plus_base' + pose_goal.pose.position.x = 0.0 pose_goal.pose.position.y = -0.09 pose_goal.pose.position.z = 0.17 @@ -124,10 +128,10 @@ def main(args=None): pose_goal.pose.orientation.y = q[1] pose_goal.pose.orientation.z = q[2] pose_goal.pose.orientation.w = q[3] - + crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' ) plan_and_execute( crane_plus, @@ -144,7 +148,7 @@ def main(args=None): pose_goal.pose.orientation.w = q[3] crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' ) plan_and_execute( crane_plus, @@ -157,7 +161,7 @@ def main(args=None): pose_goal.pose.position.z = 0.14 crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' ) plan_and_execute( crane_plus, @@ -167,7 +171,7 @@ def main(args=None): ) # Grasp - robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE]) + robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -176,12 +180,12 @@ def main(args=None): logger, single_plan_parameters=gripper_plan_request_params, ) - + # armをz軸上に動かす pose_goal.pose.position.z = 0.17 crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' ) plan_and_execute( crane_plus, @@ -191,9 +195,9 @@ def main(args=None): ) # ----- Placing Preparation ----- - # armを現在の位置から"home"ポジションに動かす + # armを現在の位置から'home'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") + crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, crane_plus_arm, @@ -212,7 +216,7 @@ def main(args=None): pose_goal.pose.orientation.w = q[3] crane_plus_arm.set_start_state_to_current_state() crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link="crane_plus_link4" + pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' ) plan_and_execute( crane_plus, @@ -222,7 +226,7 @@ def main(args=None): ) # Release - robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN]) + robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -233,9 +237,9 @@ def main(args=None): ) # Return to home and vertical pose - # armを現在の位置から"home"ポジションに動かす + # armを現在の位置から'home'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") + crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, crane_plus_arm, @@ -243,9 +247,9 @@ def main(args=None): single_plan_parameters=arm_plan_request_params, ) - # armを現在の位置から"vertical"ポジションに動かす + # armを現在の位置から'vertical'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, crane_plus_arm, @@ -254,7 +258,7 @@ def main(args=None): ) # gripperの開閉をデフォルトの間隔に戻す - robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT]) + robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -263,7 +267,7 @@ def main(args=None): logger, single_plan_parameters=gripper_plan_request_params, ) - + # MoveItPyの終了 crane_plus.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 779e463e..401c759b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -12,11 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion import datetime + +from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion + from geometry_msgs.msg import Pose + import math + import numpy as np + # moveit python library from moveit.core.robot_state import RobotState from moveit_msgs.msg import Constraints, JointConstraint @@ -51,16 +56,22 @@ def __init__(self, crane_plus): self.crane_plus, 'ompl_rrtc', ) - self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 + + # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # gripperのパラメータ設定用 self.gripper_plan_request_params = PlanRequestParameters( self.crane_plus, 'ompl_rrtc', ) - self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 + + # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # SRDFに定義されている'home'の姿勢にする self.crane_plus_arm.set_start_state_to_current_state() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index 4354a7df..25ac4ab5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -12,17 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger +from crane_plus_examples_py.utils import plan_and_execute # moveit python library from moveit.planning import ( MoveItPy, PlanRequestParameters, ) -from crane_plus_examples_py.utils import plan_and_execute +import rclpy +from rclpy.logging import get_logger def main(args=None): @@ -30,26 +28,26 @@ def main(args=None): rclpy.init(args=args) # ロガー生成 - logger = get_logger("pose_groupstate") + logger = get_logger('pose_groupstate') # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - logger.info("MoveItPy instance created") + crane_plus = MoveItPy(node_name='moveit_py') + crane_plus_arm = crane_plus.get_planning_component('arm') + logger.info('MoveItPy instance created') # planningのパラメータ設定 plan_request_params = PlanRequestParameters( crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) # 速度＆加速度のスケーリングファクタを設定 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # 現在の位置から"home"ポジションに動かす + # 現在の位置から'home'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") + crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, crane_plus_arm, @@ -57,9 +55,9 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # 現在の位置から"vertical"ポジションに動かす + # 現在の位置から'vertical'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, crane_plus_arm, @@ -67,9 +65,9 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # 現在の位置から"home"ポジションに動かす + # 現在の位置から'home'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="home") + crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, crane_plus_arm, From c122fb8b789d515c1477b03ae012daf6130cf868 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 1 Nov 2024 11:49:57 +0900 Subject: [PATCH 057/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 2 +- .../crane_plus_examples_py/color_detection.py | 2 +- .../crane_plus_examples_py/gripper_control.py | 2 +- .../crane_plus_examples_py/joint_values.py | 4 +- .../crane_plus_examples_py/pick_and_place.py | 7 +-- .../pick_and_place_tf.py | 11 ++--- .../crane_plus_examples_py/pose_groupstate.py | 4 +- .../crane_plus_examples_py/utils.py | 6 +-- .../launch/camera_example.launch.py | 43 +++++++++---------- 9 files changed, 41 insertions(+), 40 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index d886cdd2..7c719e23 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -104,5 +104,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index dd725ac2..84c94b33 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -18,8 +18,8 @@ from image_geometry import PinholeCameraModel import rclpy from rclpy.node import Node -import tf2_ros from sensor_msgs.msg import CameraInfo, Image +import tf2_ros class ImageSubscriber(Node): diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 7a8e42c3..dcf4e68c 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -92,5 +92,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index f8e06f2e..dd49b7c3 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -17,12 +17,12 @@ from crane_plus_examples_py.utils import plan_and_execute # moveit python library +from moveit.core.kinematic_constraints import construct_joint_constraint from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) -from moveit.core.kinematic_constraints import construct_joint_constraint import rclpy from rclpy.logging import get_logger @@ -91,5 +91,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 7ab7e49d..94678781 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -12,8 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute from geometry_msgs.msg import PoseStamped + import math # moveit python library @@ -66,7 +67,7 @@ def main(args=None): gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # gripperの開閉角度 GRIPPER_DEFAULT = 0.0 @@ -275,5 +276,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 401c759b..378d59fd 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -14,13 +14,11 @@ import datetime -from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion - -from geometry_msgs.msg import Pose +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute import math -import numpy as np +from geometry_msgs.msg import Pose # moveit python library from moveit.core.robot_state import RobotState @@ -29,12 +27,15 @@ MoveItPy, PlanRequestParameters, ) + +import numpy as np + import rclpy from rclpy.logging import get_logger from rclpy.node import Node from rclpy.time import Time import tf2_ros -from tf2_ros import TransformStamped, TransformListener +from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index 25ac4ab5..bdafc6d0 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -40,7 +40,7 @@ def main(args=None): crane_plus, 'ompl_rrtc', ) - + # 速度＆加速度のスケーリングファクタを設定 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 @@ -82,5 +82,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 3b48f781..b3b88339 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -13,8 +13,11 @@ # limitations under the License. import numpy as np + from rclpy.logging import get_logger + import time + import quaternion @@ -35,15 +38,12 @@ def plan_and_execute( plan_result = planning_component.plan( multi_plan_parameters=multi_plan_parameters ) - logger.info('plan_result >>>>> multi', once=True) elif single_plan_parameters is not None: plan_result = planning_component.plan( single_plan_parameters=single_plan_parameters ) - logger.info('plan_result >>>>> single', once=True) else: plan_result = planning_component.plan() - logger.info('plan_result >>>>> none', once=True) # execute the plan result = None diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index 3fdc3ef0..05733f36 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -15,7 +15,6 @@ import os from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -25,41 +24,41 @@ def generate_launch_description(): moveit_config = ( - MoveItConfigsBuilder("crane_plus") + MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) .robot_description( file_path=os.path.join( - get_package_share_directory("crane_plus_description"), - "urdf", - "crane_plus.urdf.xacro", + get_package_share_directory('crane_plus_description'), + 'urdf', + 'crane_plus.urdf.xacro', ), mappings={}, ) .robot_description_semantic( - file_path="config/crane_plus.srdf", - mappings={"model": "crane_plus"}, + file_path='config/crane_plus.srdf', + mappings={'model': 'crane_plus'}, ) - .joint_limits(file_path="config/joint_limits.yaml") + .joint_limits(file_path='config/joint_limits.yaml') .trajectory_execution( - file_path="config/controllers.yaml", moveit_manage_controllers=True + file_path='config/controllers.yaml', moveit_manage_controllers=True ) - .planning_pipelines(pipelines=["ompl"]) - .robot_description_kinematics(file_path="config/kinematics.yaml") + .planning_pipelines(pipelines=['ompl']) + .robot_description_kinematics(file_path='config/kinematics.yaml') .moveit_cpp( - file_path=get_package_share_directory("crane_plus_examples_py") - + "/config/crane_plus_moveit_py_examples.yaml" + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' ) .to_moveit_configs() ) moveit_config.robot_description = { - "robot_description": LaunchConfiguration("loaded_description") + 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {"capabilities": ""} + moveit_config.move_group_capabilities = {'capabilities': ''} declare_example_name = DeclareLaunchArgument( 'example', default_value='color_detection', @@ -69,21 +68,21 @@ def generate_launch_description(): picking_node = Node( name='pick_and_place_tf', - package="crane_plus_examples_py", + package='crane_plus_examples_py', executable='pick_and_place_tf', - output="screen", + output='screen', parameters=[ moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, ], ) - + detection_node = Node( - name=[LaunchConfiguration("example"), "_node"], - package="crane_plus_examples_py", - executable=LaunchConfiguration("example"), - output="screen" + name=[LaunchConfiguration('example'), '_node'], + package='crane_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen' ) ld = LaunchDescription() From 1e1c7e2378b2f3c14d9af05093dc0439aae78a98 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 1 Nov 2024 12:04:05 +0900 Subject: [PATCH 058/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/joint_values.py | 2 +- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/utils.py | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index dd49b7c3..dbca46c5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -52,7 +52,7 @@ def main(args=None): plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # armの関節のjoint1〜4を順番に45[deg]ずつ動かす - joint_names = ['crane_plus_joint1', 'crane_plus_joint2', + joint_names = ['crane_plus_joint1', 'crane_plus_joint2', 'crane_plus_joint3', 'crane_plus_joint4'] target_joint_value = math.radians(45) for joint_name in joint_names: diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 378d59fd..8ca5a774 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -140,7 +140,7 @@ def on_timer(self): else: TF_STOP_TIME = now.nanoseconds - # 現在時刻から2秒以内に受け取ったtfを使用 + # 現在時刻から2秒以内に受け取ったtfを使用 if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: tf_diff = np.sqrt((self.tf_past.transform.translation.x - tf_msg.transform.translation.x) ** 2 diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index b3b88339..7c0089e4 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -12,12 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +import time + import numpy as np from rclpy.logging import get_logger -import time - import quaternion @@ -30,7 +30,7 @@ def plan_and_execute( multi_plan_parameters=None, sleep_time=0.0, ): - """Helper function to plan and execute a motion.""" + """Plan and execute a motion.""" # plan to goal logger = get_logger('plan_and_execute') logger.info('Planning trajectory') From 1a5fd93fa121709831533dd2b3cab849d928802c Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 1 Nov 2024 12:19:15 +0900 Subject: [PATCH 059/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 4 +- .../pick_and_place_tf.py | 5 +- .../crane_plus_examples_py/utils.py | 4 +- .../launch/example.launch.py | 70 +++++++++---------- 4 files changed, 40 insertions(+), 43 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index dcf4e68c..8dbe5906 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import plan_and_execute - import math # moveit python library @@ -25,6 +23,8 @@ import rclpy from rclpy.logging import get_logger +from crane_plus_examples_py.utils import plan_and_execute + def main(args=None): # ros2の初期化 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 8ca5a774..b07d501b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -13,9 +13,6 @@ # limitations under the License. import datetime - -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute - import math from geometry_msgs.msg import Pose @@ -38,6 +35,8 @@ from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + class PickAndPlaceTf(Node): def __init__(self, crane_plus): diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 7c0089e4..48a22578 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -13,13 +13,11 @@ # limitations under the License. import time - import numpy as np +import quaternion from rclpy.logging import get_logger -import quaternion - # plan and execute def plan_and_execute( diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index e5339656..70e675e0 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -26,87 +26,87 @@ def generate_launch_description(): # declare_loaded_description = DeclareLaunchArgument( - # "loaded_description", - # default_value="", - # description="Set robot_description text. \ - # It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + # "loaded_description', + # default_value='', + # description='Set robot_description text. \ + # It is recommended to use RobotDescriptionLoader() in crane_plus_description.', # ) ld = LaunchDescription() description_loader = RobotDescriptionLoader() ld.add_action( DeclareLaunchArgument( - "loaded_description", + 'loaded_description', default_value=description_loader.load(), - description="Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.", + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description.', ) ) moveit_config = ( - MoveItConfigsBuilder("crane_plus") + MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) .robot_description( file_path=os.path.join( - get_package_share_directory("crane_plus_description"), - "urdf", - "crane_plus.urdf.xacro", + get_package_share_directory('crane_plus_description'), + 'urdf', + 'crane_plus.urdf.xacro', ), mappings={}, ) .robot_description_kinematics( - file_path=get_package_share_directory("crane_plus_moveit_config") - + "/config/kinematics.yaml" + file_path=get_package_share_directory('crane_plus_moveit_config') + + '/config/kinematics.yaml' ) # .robot_description_semantic( - # file_path="config/crsane_plus.srdf", - # mappings={"model": "crane_plus"}, + # file_path='config/crsane_plus.srdf', + # mappings={'model': 'crane_plus'}, # ) - # .joint_limits(file_path="config/joint_limits.yaml") + # .joint_limits(file_path='config/joint_limits.yaml') .trajectory_execution( - file_path=get_package_share_directory("crane_plus_moveit_config") - + "/config/controllers.yaml" + file_path=get_package_share_directory('crane_plus_moveit_config') + + '/config/controllers.yaml' ) - # .planning_pipelines(pipelines=["ompl"]) + # .planning_pipelines(pipelines=['ompl']) .moveit_cpp( - file_path=get_package_share_directory("crane_plus_examples_py") - + "/config/crane_plus_moveit_py_examples.yaml" + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' ) .to_moveit_configs() ) moveit_config.robot_description = { - "robot_description": LaunchConfiguration("loaded_description") + 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {"capabilities": ""} + moveit_config.move_group_capabilities = {'capabilities': ''} declare_example_name = DeclareLaunchArgument( - "example", - default_value="gripper_control", + 'example', + default_value='gripper_control', description=( - "Set an example executable name: " - "[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]" + 'Set an example executable name: ' + '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' ), ) # declare_use_sim_time = DeclareLaunchArgument( - # "use_sim_time", - # default_value="false", - # description=("Set true when using the gazebo simulator."), + # 'use_sim_time', + # default_value='false', + # description=('Set true when using the gazebo simulator.'), # ) # use_sim_time_name = SetParameter( - # name="use_sim_time", value=LaunchConfiguration("use_sim_time") + # name='use_sim_time', value=LaunchConfiguration('use_sim_time') # ) example_node = Node( - name=[LaunchConfiguration("example"), "_node"], - package="crane_plus_examples_py", - executable=LaunchConfiguration("example"), - output="screen", + name=[LaunchConfiguration('example'), '_node'], + package='crane_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen', parameters=[moveit_config.to_dict()], ) From 32a0cb8315dfc7513be0fc56c980d9f2df2f9d61 Mon Sep 17 00:00:00 2001 From: mizonon Date: Fri, 1 Nov 2024 12:35:39 +0900 Subject: [PATCH 060/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 2 +- .../crane_plus_examples_py/pick_and_place.py | 7 ++++--- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/utils.py | 1 + 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 8dbe5906..67f13e5b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -21,9 +21,9 @@ PlanRequestParameters, ) import rclpy -from rclpy.logging import get_logger from crane_plus_examples_py.utils import plan_and_execute +from rclpy.logging import get_logger def main(args=None): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 94678781..a59dd521 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -12,11 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute -from geometry_msgs.msg import PoseStamped - import math +from geometry_msgs.msg import PoseStamped + # moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( @@ -26,6 +25,8 @@ import rclpy from rclpy.logging import get_logger +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + def main(args=None): # ros2の初期化 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index b07d501b..ab20a930 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -19,11 +19,11 @@ # moveit python library from moveit.core.robot_state import RobotState -from moveit_msgs.msg import Constraints, JointConstraint from moveit.planning import ( MoveItPy, PlanRequestParameters, ) +from moveit_msgs.msg import Constraints, JointConstraint import numpy as np diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 48a22578..722a97e4 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -13,6 +13,7 @@ # limitations under the License. import time + import numpy as np import quaternion From 1ff1b3bdfa9ca0fec397e572e20f8a25248d6631 Mon Sep 17 00:00:00 2001 From: mizonon Date: Mon, 11 Nov 2024 15:47:07 +0900 Subject: [PATCH 061/111] =?UTF-8?q?=E3=82=AB=E3=83=A1=E3=83=A9=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E7=94=A8=E3=81=AElaunch=E3=83=95?= =?UTF-8?q?=E3=82=A1=E3=82=A4=E3=83=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/camera_example.launch.py | 36 +++++++++++++------ 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index 05733f36..b9ce5556 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -23,6 +23,18 @@ def generate_launch_description(): + ld = LaunchDescription() + + # declare_loaded_description = DeclareLaunchArgument( + # 'loaded_description', + # default_value='', + # description='Set robot_description text. \ + # It is recommended to use RobotDescriptionLoader() in \ + # crane_plus_description.', + # ) + + # ld.add_action(declare_loaded_description) + moveit_config = ( MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( @@ -37,16 +49,20 @@ def generate_launch_description(): ), mappings={}, ) - .robot_description_semantic( - file_path='config/crane_plus.srdf', - mappings={'model': 'crane_plus'}, + .robot_description_kinematics( + file_path=get_package_share_directory('crane_plus_moveit_config') + + '/config/kinematics.yaml' ) - .joint_limits(file_path='config/joint_limits.yaml') + # .robot_description_semantic( + # file_path='config/crsane_plus.srdf', + # mappings={'model': 'crane_plus'}, + # ) + # .joint_limits(file_path='config/joint_limits.yaml') .trajectory_execution( - file_path='config/controllers.yaml', moveit_manage_controllers=True + file_path=get_package_share_directory('crane_plus_moveit_config') + + '/config/controllers.yaml' ) - .planning_pipelines(pipelines=['ompl']) - .robot_description_kinematics(file_path='config/kinematics.yaml') + # .planning_pipelines(pipelines=['ompl']) .moveit_cpp( file_path=get_package_share_directory('crane_plus_examples_py') + '/config/crane_plus_moveit_py_examples.yaml' @@ -78,15 +94,15 @@ def generate_launch_description(): ], ) - detection_node = Node( + example_node = Node( name=[LaunchConfiguration('example'), '_node'], package='crane_plus_examples_py', executable=LaunchConfiguration('example'), output='screen' ) - ld = LaunchDescription() - ld.add_action(detection_node) + # ld = LaunchDescription() + ld.add_action(example_node) ld.add_action(picking_node) ld.add_action(declare_example_name) From c823afa913aaedf675cab6c9bc606f1acca925df Mon Sep 17 00:00:00 2001 From: mizonon Date: Mon, 11 Nov 2024 18:28:49 +0900 Subject: [PATCH 062/111] =?UTF-8?q?=E3=82=AB=E3=83=A1=E3=83=A9=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E5=8B=95=E4=BD=9C=E7=A2=BA=E8=AA=8D?= =?UTF-8?q?=E7=94=A8=E3=81=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../pick_and_place_tf.py | 21 ++++++++------- .../launch/camera_example.launch.py | 26 ++++++++++++++----- 2 files changed, 31 insertions(+), 16 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index ab20a930..68ea7ea5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -15,7 +15,7 @@ import datetime import math -from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped # moveit python library from moveit.core.robot_state import RobotState @@ -117,6 +117,7 @@ def __init__(self, crane_plus): def on_timer(self): # target_0のtf位置姿勢を取得 + tf_msg = TransformStamped() try: tf_msg = self.tf_buffer.lookup_transform( 'base_link', 'target_0', Time()) @@ -219,16 +220,18 @@ def _control_gripper(self, angle): # アーム制御 def _control_arm(self, x, y, z, roll, pitch, yaw): - target_pose = Pose() - target_pose.position.x = x - target_pose.position.y = y - target_pose.position.z = z + # target_pose = Pose() + target_pose = PoseStamped() + target_pose.header.frame_id = 'crane_plus_base' + target_pose.pose.position.x = x + target_pose.pose.position.y = y + target_pose.pose.position.z = z q = euler_to_quaternion(math.radians(roll), math.radians(pitch), math.radians(yaw)) - target_pose.orientation.x = q[0] - target_pose.orientation.y = q[1] - target_pose.orientation.z = q[2] - target_pose.orientation.w = q[3] + target_pose.pose.orientation.x = q[0] + target_pose.pose.orientation.y = q[1] + target_pose.pose.orientation.z = q[2] + target_pose.pose.orientation.w = q[3] self.crane_plus_arm.set_start_state_to_current_state() self.crane_plus_arm.set_goal_state( pose_stamped_msg=target_pose, pose_link='crane_plus_link4' diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index b9ce5556..e42d9cc6 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -15,6 +15,7 @@ import os from ament_index_python.packages import get_package_share_directory +from crane_plus_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -23,8 +24,7 @@ def generate_launch_description(): - ld = LaunchDescription() - + # declare_loaded_description = DeclareLaunchArgument( # 'loaded_description', # default_value='', @@ -33,6 +33,16 @@ def generate_launch_description(): # crane_plus_description.', # ) + ld = LaunchDescription() + description_loader = RobotDescriptionLoader() + ld.add_action( + DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in crane_plus_description.', + ) + ) # ld.add_action(declare_loaded_description) moveit_config = ( @@ -79,7 +89,7 @@ def generate_launch_description(): declare_example_name = DeclareLaunchArgument( 'example', default_value='color_detection', description=('Set an example executable name: ' - '[color_detection]') + '[aruco_detection, color_detection]') ) picking_node = Node( @@ -88,9 +98,10 @@ def generate_launch_description(): executable='pick_and_place_tf', output='screen', parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.robot_description_kinematics, + # moveit_config.robot_description, + # moveit_config.robot_description_semantic, + # moveit_config.robot_description_kinematics, + moveit_config.to_dict() ], ) @@ -98,7 +109,8 @@ def generate_launch_description(): name=[LaunchConfiguration('example'), '_node'], package='crane_plus_examples_py', executable=LaunchConfiguration('example'), - output='screen' + output='screen', + parameters=[moveit_config.to_dict()], ) # ld = LaunchDescription() From dc07458f5f1f8197e95133ed0501c1f6a4e03a28 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 14:37:41 +0900 Subject: [PATCH 063/111] =?UTF-8?q?=E3=82=AB=E3=83=A1=E3=83=A9=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E7=94=A8=E3=81=AElaunch=E3=83=95?= =?UTF-8?q?=E3=82=A1=E3=82=A4=E3=83=AB=E5=BE=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/camera_example.launch.py | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index e42d9cc6..e92488d4 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -15,7 +15,8 @@ import os from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader import RobotDescriptionLoader +from crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -24,15 +25,6 @@ def generate_launch_description(): - - # declare_loaded_description = DeclareLaunchArgument( - # 'loaded_description', - # default_value='', - # description='Set robot_description text. \ - # It is recommended to use RobotDescriptionLoader() in \ - # crane_plus_description.', - # ) - ld = LaunchDescription() description_loader = RobotDescriptionLoader() ld.add_action( @@ -40,10 +32,10 @@ def generate_launch_description(): 'loaded_description', default_value=description_loader.load(), description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.', + It is recommended to use RobotDescriptionLoader() \ + in crane_plus_description.', ) ) - # ld.add_action(declare_loaded_description) moveit_config = ( MoveItConfigsBuilder('crane_plus') From f4161fdc97eb65346ae48dca4e703a4e6e70996b Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 14:46:03 +0900 Subject: [PATCH 064/111] =?UTF-8?q?crane=5Fplush=5Fexamples=5Fpy=E3=81=AE?= =?UTF-8?q?=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E8=B5=B7=E5=8B=95=E7=94=A8?= =?UTF-8?q?=E3=81=AElaunch=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB=E5=BE=AE?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/launch/example.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index 70e675e0..c9bb64b9 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -38,7 +38,8 @@ def generate_launch_description(): 'loaded_description', default_value=description_loader.load(), description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_plus_description.', + It is recommended to use RobotDescriptionLoader() \ + in crane_plus_description.', ) ) From 1850f69a9b330fcfcc08ac601baf95221721a461 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 15:16:18 +0900 Subject: [PATCH 065/111] =?UTF-8?q?crane=5Fplus=5Fexamples=5Fpy=E3=81=AEpi?= =?UTF-8?q?ck=5Fand=5Fplace=5Ftf.py=E5=BE=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place_tf.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 68ea7ea5..0a0b2fe5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -79,7 +79,6 @@ def __init__(self, crane_plus): plan_and_execute( self.crane_plus, self.crane_plus_arm, - # logger=None, self.logger, single_plan_parameters=self.arm_plan_request_params, ) @@ -213,14 +212,12 @@ def _control_gripper(self, angle): plan_and_execute( self.crane_plus, self.crane_plus_gripper, - # logger=None, self.logger, single_plan_parameters=self.gripper_plan_request_params, ) # アーム制御 def _control_arm(self, x, y, z, roll, pitch, yaw): - # target_pose = Pose() target_pose = PoseStamped() target_pose.header.frame_id = 'crane_plus_base' target_pose.pose.position.x = x From 225a60444d78e885ea3f8f162fdec02972a6d8b4 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 16:52:37 +0900 Subject: [PATCH 066/111] =?UTF-8?q?crane=5Fplus=5Fexamples=5Fpy/utils.py?= =?UTF-8?q?=E3=81=AE=E4=BD=99=E8=A8=88=E3=81=AA=E3=82=B3=E3=83=A1=E3=83=B3?= =?UTF-8?q?=E3=83=88=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/crane_plus_examples_py/utils.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 722a97e4..e02e9553 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,11 +1,8 @@ # Copyright 2020 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. @@ -29,7 +26,6 @@ def plan_and_execute( multi_plan_parameters=None, sleep_time=0.0, ): - """Plan and execute a motion.""" # plan to goal logger = get_logger('plan_and_execute') logger.info('Planning trajectory') From 1f59bd9bef833715f6362604fa751e7fd9ab20cc Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 17:26:23 +0900 Subject: [PATCH 067/111] =?UTF-8?q?crane=5Fplus=5Fexamples=5Fpy=E3=81=AE?= =?UTF-8?q?=E5=90=84=E3=82=B3=E3=83=BC=E3=83=89=E5=BE=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 3 ++- .../crane_plus_examples_py/pick_and_place.py | 4 +++- .../crane_plus_examples_py/pick_and_place_tf.py | 4 ++-- crane_plus_examples_py/launch/example.launch.py | 8 +++++--- 4 files changed, 12 insertions(+), 7 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 67f13e5b..50240bac 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -20,10 +20,11 @@ MoveItPy, PlanRequestParameters, ) -import rclpy from crane_plus_examples_py.utils import plan_and_execute + from rclpy.logging import get_logger +import rclpy def main(args=None): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index a59dd521..9b1eda66 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -22,10 +22,12 @@ MoveItPy, PlanRequestParameters, ) + +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + import rclpy from rclpy.logging import get_logger -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute def main(args=None): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 0a0b2fe5..55eccf8b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -27,6 +27,8 @@ import numpy as np +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + import rclpy from rclpy.logging import get_logger from rclpy.node import Node @@ -35,8 +37,6 @@ from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute - class PickAndPlaceTf(Node): def __init__(self, crane_plus): diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index c9bb64b9..68ca131a 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -15,12 +15,13 @@ import os from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader import RobotDescriptionLoader +from crane_plus_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 Node -from launch_ros.actions import SetParameter +# from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder @@ -89,7 +90,8 @@ def generate_launch_description(): default_value='gripper_control', description=( 'Set an example executable name: ' - '[gripper_control, pose_groupstate, joint_values, pick_and_place, pick_and_place_tf]' + '[gripper_control, pose_groupstate, \ + joint_values, pick_and_place, pick_and_place_tf]' ), ) From 78dbc3c37e1425107fad280c7c8d2414f83906a3 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 17:39:15 +0900 Subject: [PATCH 068/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C=E3=81=AE=E5=BE=AE?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 6 +++--- .../crane_plus_examples_py/pick_and_place.py | 1 - .../crane_plus_examples_py/pick_and_place_tf.py | 4 ++-- crane_plus_examples_py/launch/example.launch.py | 6 ++++-- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 50240bac..548c0554 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -14,6 +14,8 @@ import math +from crane_plus_examples_py.utils import plan_and_execute + # moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( @@ -21,10 +23,8 @@ PlanRequestParameters, ) -from crane_plus_examples_py.utils import plan_and_execute - -from rclpy.logging import get_logger import rclpy +from rclpy.logging import get_logger def main(args=None): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 9b1eda66..ef1fda1b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -29,7 +29,6 @@ from rclpy.logging import get_logger - def main(args=None): # ros2の初期化 rclpy.init(args=args) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 55eccf8b..95bb1f41 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -15,6 +15,8 @@ import datetime import math +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + from geometry_msgs.msg import PoseStamped # moveit python library @@ -27,8 +29,6 @@ import numpy as np -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute - import rclpy from rclpy.logging import get_logger from rclpy.node import Node diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index 68ca131a..1abadb16 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -30,7 +30,8 @@ def generate_launch_description(): # "loaded_description', # default_value='', # description='Set robot_description text. \ - # It is recommended to use RobotDescriptionLoader() in crane_plus_description.', + # It is recommended to use RobotDescriptionLoader() in \ + # crane_plus_description.', # ) ld = LaunchDescription() description_loader = RobotDescriptionLoader() @@ -113,7 +114,8 @@ def generate_launch_description(): parameters=[moveit_config.to_dict()], ) - # ld = LaunchDescription([SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time'))]) + # ld = LaunchDescription([SetParameter( + # name='use_sim_time', value=LaunchConfiguration('use_sim_time'))]) # ld.add_action(declare_use_sim_time) # ld.add_action(use_sim_time_name) # ld.add_action(declare_loaded_description) From 9b161aac228d2872e9107b8fe53557293c7ae1e5 Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 17:47:26 +0900 Subject: [PATCH 069/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C=E3=81=AE=E5=BE=AE?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index ef1fda1b..861fdcf5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -14,6 +14,8 @@ import math +from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute + from geometry_msgs.msg import PoseStamped # moveit python library From 2361a41932dea52acc5d36fc528f97140a97d64d Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 17:55:09 +0900 Subject: [PATCH 070/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C=E3=81=AE=E5=BE=AE?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 861fdcf5..b9ed4885 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -25,8 +25,6 @@ PlanRequestParameters, ) -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute - import rclpy from rclpy.logging import get_logger From 5d05398f2a673ad0a95c9579319e48485284f7dc Mon Sep 17 00:00:00 2001 From: mizonon Date: Tue, 12 Nov 2024 18:04:24 +0900 Subject: [PATCH 071/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C=E3=81=AE=E5=BE=AE?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index fcb6446b..567c4009 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -1,5 +1,6 @@ from glob import glob import os + from setuptools import find_packages, setup package_name = 'crane_plus_examples_py' From f797d08342c11975db42169f98f13d5fdbd7d2a8 Mon Sep 17 00:00:00 2001 From: mizonon Date: Wed, 20 Nov 2024 15:06:25 +0900 Subject: [PATCH 072/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 567c4009..5b7effba 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -1,5 +1,5 @@ -from glob import glob import os +from glob import glob from setuptools import find_packages, setup From 8daa08ffe750956ca8610451373e0009f207c700 Mon Sep 17 00:00:00 2001 From: mizonon Date: Wed, 20 Nov 2024 15:23:58 +0900 Subject: [PATCH 073/111] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 5b7effba..567c4009 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -1,5 +1,5 @@ -import os from glob import glob +import os from setuptools import find_packages, setup From 2b4cdd801b16ccaa463a8cdba407b0b48e8931b8 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 27 Dec 2024 13:47:01 +0900 Subject: [PATCH 074/111] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_control/launch/crane_plus_control.launch.py | 2 +- crane_plus_description/launch/display.launch.py | 2 +- crane_plus_examples/launch/demo.launch.py | 2 +- crane_plus_examples/launch/example.launch.py | 2 +- .../crane_plus_examples_py/aruco_detection.py | 2 +- .../crane_plus_examples_py/color_detection.py | 2 +- .../crane_plus_examples_py/gripper_control.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/joint_values.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py | 2 +- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- .../crane_plus_examples_py/pose_groupstate.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/utils.py | 2 +- crane_plus_moveit_config/launch/run_move_group.launch.py | 2 +- crane_plus_moveit_config/launch/setup_assistant.launch.py | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/crane_plus_control/launch/crane_plus_control.launch.py b/crane_plus_control/launch/crane_plus_control.launch.py index 4eac1d66..0b885676 100644 --- a/crane_plus_control/launch/crane_plus_control.launch.py +++ b/crane_plus_control/launch/crane_plus_control.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_description/launch/display.launch.py b/crane_plus_description/launch/display.launch.py index 416c01a0..da47f454 100644 --- a/crane_plus_description/launch/display.launch.py +++ b/crane_plus_description/launch/display.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 1099c8bb..533a259a 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples/launch/example.launch.py b/crane_plus_examples/launch/example.launch.py index e5f98740..95563c3d 100644 --- a/crane_plus_examples/launch/example.launch.py +++ b/crane_plus_examples/launch/example.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 7c719e23..b3ffa174 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index 84c94b33..08c39601 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 548c0554..e3e8f7a7 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index dbca46c5..570fc261 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index b9ed4885..96e4d5cc 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 95bb1f41..25b87d32 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index bdafc6d0..03031e22 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index e02e9553..9f890088 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 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 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 0a8ec0b8..4116cd0a 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py index 77eb4d9f..15432c78 100755 --- a/crane_plus_moveit_config/launch/setup_assistant.launch.py +++ b/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From e342fbb3fd8e29f323b175ff5be48a286a0ea593 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 27 Dec 2024 14:06:36 +0900 Subject: [PATCH 075/111] =?UTF-8?q?=E9=80=9A=E4=BF=A1=E3=82=A8=E3=83=A9?= =?UTF-8?q?=E3=83=BC=E6=99=82=E3=81=AE=E5=87=A6=E7=90=86=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_control/src/crane_plus_hardware.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index 7a58954e..98aad53c 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -205,6 +205,9 @@ return_type CranePlusHardware::read( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + // readに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } else { for (uint i = 0; i < hw_position_states_.size(); ++i) { hw_position_states_[i] = joint_positions[i]; @@ -270,6 +273,9 @@ return_type CranePlusHardware::write( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + // writeに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } prev_comm_timestamp_ = steady_clock_.now(); From 20aaa8e9907e9f0ac65e4f4a22f2b3c1a44d3d50 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 27 Dec 2024 14:11:00 +0900 Subject: [PATCH 076/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E5=B7=AE?= =?UTF-8?q?=E5=88=86=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_plus_moveit_config/CMakeLists.txt b/crane_plus_moveit_config/CMakeLists.txt index d6392526..1227603e 100644 --- a/crane_plus_moveit_config/CMakeLists.txt +++ b/crane_plus_moveit_config/CMakeLists.txt @@ -13,4 +13,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() \ No newline at end of file +ament_package() From 6614b2ad2a27fc4e529886961ba99939cbb30869 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 27 Dec 2024 14:13:50 +0900 Subject: [PATCH 077/111] =?UTF-8?q?Revert=20"=E3=82=B3=E3=83=A1=E3=83=B3?= =?UTF-8?q?=E3=83=88=E4=BF=AE=E6=AD=A3"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit 2b4cdd801b16ccaa463a8cdba407b0b48e8931b8. --- crane_plus_control/launch/crane_plus_control.launch.py | 2 +- crane_plus_description/launch/display.launch.py | 2 +- crane_plus_examples/launch/demo.launch.py | 2 +- crane_plus_examples/launch/example.launch.py | 2 +- .../crane_plus_examples_py/aruco_detection.py | 2 +- .../crane_plus_examples_py/color_detection.py | 2 +- .../crane_plus_examples_py/gripper_control.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/joint_values.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py | 2 +- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- .../crane_plus_examples_py/pose_groupstate.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/utils.py | 2 +- crane_plus_moveit_config/launch/run_move_group.launch.py | 2 +- crane_plus_moveit_config/launch/setup_assistant.launch.py | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/crane_plus_control/launch/crane_plus_control.launch.py b/crane_plus_control/launch/crane_plus_control.launch.py index 0b885676..4eac1d66 100644 --- a/crane_plus_control/launch/crane_plus_control.launch.py +++ b/crane_plus_control/launch/crane_plus_control.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_description/launch/display.launch.py b/crane_plus_description/launch/display.launch.py index da47f454..416c01a0 100644 --- a/crane_plus_description/launch/display.launch.py +++ b/crane_plus_description/launch/display.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 533a259a..1099c8bb 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples/launch/example.launch.py b/crane_plus_examples/launch/example.launch.py index 95563c3d..e5f98740 100644 --- a/crane_plus_examples/launch/example.launch.py +++ b/crane_plus_examples/launch/example.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index b3ffa174..7c719e23 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index 08c39601..84c94b33 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index e3e8f7a7..548c0554 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 570fc261..dbca46c5 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 96e4d5cc..b9ed4885 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 25b87d32..95bb1f41 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index 03031e22..bdafc6d0 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 9f890088..e02e9553 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 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 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 4116cd0a..0a8ec0b8 100755 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_moveit_config/launch/setup_assistant.launch.py b/crane_plus_moveit_config/launch/setup_assistant.launch.py index 15432c78..77eb4d9f 100755 --- a/crane_plus_moveit_config/launch/setup_assistant.launch.py +++ b/crane_plus_moveit_config/launch/setup_assistant.launch.py @@ -1,4 +1,4 @@ -# Copyright 2025 RT Corporation +# Copyright 2020 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. From 37778b1e7687a68e3d1cd5522516076cff027446 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 27 Dec 2024 14:16:26 +0900 Subject: [PATCH 078/111] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 2 +- .../crane_plus_examples_py/color_detection.py | 2 +- .../crane_plus_examples_py/gripper_control.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/joint_values.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py | 2 +- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- .../crane_plus_examples_py/pose_groupstate.py | 2 +- crane_plus_examples_py/crane_plus_examples_py/utils.py | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 7c719e23..b3ffa174 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index 84c94b33..08c39601 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index 548c0554..e3e8f7a7 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index dbca46c5..570fc261 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index b9ed4885..96e4d5cc 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 95bb1f41..25b87d32 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index bdafc6d0..03031e22 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index e02e9553..9f890088 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,4 +1,4 @@ -# Copyright 2020 RT Corporation +# Copyright 2025 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 From 6c52b25771d2c4295a1744ccb9c0c1802e7a1836 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 27 Dec 2024 14:17:22 +0900 Subject: [PATCH 079/111] =?UTF-8?q?=E3=82=A4=E3=83=B3=E3=83=87=E3=83=B3?= =?UTF-8?q?=E3=83=88=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_control/src/crane_plus_hardware.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index 98aad53c..6514e000 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -205,9 +205,9 @@ return_type CranePlusHardware::read( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - // readに失敗しても通信は継続させる。 - // 不確かなデータをセットしないようにOKを返す。 - return return_type::OK; + // readに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } else { for (uint i = 0; i < hw_position_states_.size(); ++i) { hw_position_states_[i] = joint_positions[i]; From 5a4277fdfdec6eb92c35a30ad55029458be290d7 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Tue, 7 Jan 2025 15:32:02 +0900 Subject: [PATCH 080/111] =?UTF-8?q?=E3=83=91=E3=83=83=E3=82=B1=E3=83=BC?= =?UTF-8?q?=E3=82=B8=E6=83=85=E5=A0=B1=E6=9B=B4=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/package.xml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index a44f9610..bc22daec 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -2,17 +2,21 @@ crane_plus_examples_py - 0.0.0 - TODO: Package description - root + 0.1.0 + Python examples of CRANE+ V2 ROS package + RT Corporation Apache-2.0 + Nozomi Mizoguchi + Atsushi Kuwagata + rclpy std_msgs moveit moveit_py python3-pip python-numpy-quaternion-pip + ament_copyright ament_flake8 ament_pep257 From 43c5e39181f973993764265afeffdcb19e8caf2e Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 17 Jan 2025 13:03:39 +0900 Subject: [PATCH 081/111] =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=87=E3=82=A3?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB=E3=82=92?= =?UTF-8?q?=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/gripper_control.py | 55 ++-- .../crane_plus_examples_py/joint_values.py | 46 ++-- .../crane_plus_examples_py/pick_and_place.py | 247 ++++++++---------- .../crane_plus_examples_py/utils.py | 42 +-- .../launch/example.launch.py | 97 ++----- crane_plus_examples_py/package.xml | 4 +- 6 files changed, 190 insertions(+), 301 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py index e3e8f7a7..38867001 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -16,7 +16,6 @@ from crane_plus_examples_py.utils import plan_and_execute -# moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, @@ -28,68 +27,66 @@ def main(args=None): - # ros2の初期化 rclpy.init(args=args) - - # ロガー生成 logger = get_logger('gripper_control') - # MoveItPy初期化 - crane_plus = MoveItPy(node_name='moveit_py') - crane_plus_gripper = crane_plus.get_planning_component('gripper') + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='gripper_control') logger.info('MoveItPy instance created') + # グリッパ制御用 planning component + gripper = crane_plus.get_planning_component('gripper') + # instantiate a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() - robot_state = RobotState(robot_model) - # planningのパラメータ設定 plan_request_params = PlanRequestParameters( crane_plus, 'ompl_rrtc', ) - # 速度＆加速度のスケーリングファクタを設定 + # 動作速度の調整 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # グリッパーを+30[deg]の位置に動かす - robot_state.set_joint_group_positions('gripper', [math.radians(30)]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + # gripperを閉じる + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(30.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=plan_request_params, ) - # グリッパーを-30[deg]の位置に動かす - robot_state.set_joint_group_positions('gripper', [math.radians(-30)]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + # gripperを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(-30.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=plan_request_params, ) - # グリッパーを0[deg]の位置に動かす - robot_state.set_joint_group_positions('gripper', [0.0]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + # gripperを0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(0.0)]) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=plan_request_params, ) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 570fc261..73058387 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -16,30 +16,29 @@ from crane_plus_examples_py.utils import plan_and_execute -# moveit python library from moveit.core.kinematic_constraints import construct_joint_constraint from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) + import rclpy from rclpy.logging import get_logger def main(args=None): - # ros2の初期化 rclpy.init(args=args) - - # ロガー生成 logger = get_logger('joint_values') - # MoveItPy初期化 - crane_plus = MoveItPy(node_name='moveit_py') - crane_plus_arm = crane_plus.get_planning_component('arm') + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='joint_values') logger.info('MoveItPy instance created') - # instantiate a a RobotState instance using the current robot model + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm') + + # instantiate a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() robot_state = RobotState(robot_model) @@ -48,6 +47,7 @@ def main(args=None): 'ompl_rrtc', ) + # 動作速度の調整 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 @@ -55,39 +55,39 @@ def main(args=None): joint_names = ['crane_plus_joint1', 'crane_plus_joint2', 'crane_plus_joint3', 'crane_plus_joint4'] target_joint_value = math.radians(45) + for joint_name in joint_names: + arm.set_start_state_to_current_state() + joint_values = {joint_name: target_joint_value} robot_state.joint_positions = joint_values joint_constraint = construct_joint_constraint( robot_state=robot_state, - joint_model_group=crane_plus - .get_robot_model().get_joint_model_group('arm'), + joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm'), ) - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( + + arm.set_goal_state( motion_plan_constraints=[joint_constraint] ) plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=plan_request_params ) - # 現在の位置から'vertical'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='vertical') + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, - single_plan_parameters=plan_request_params, + single_plan_parameters=plan_request_params ) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 96e4d5cc..2094dcde 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -14,11 +14,8 @@ import math -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute - -from geometry_msgs.msg import PoseStamped - -# moveit python library +from crane_plus_examples_py.utils import plan_and_execute +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, @@ -27,254 +24,226 @@ import rclpy from rclpy.logging import get_logger +from scipy.spatial.transform import Rotation as R def main(args=None): - # ros2の初期化 rclpy.init(args=args) - - # ロガー生成 logger = get_logger('pick_and_place') - # MoveItPy初期化 - crane_plus = MoveItPy(node_name='moveit_py') - crane_plus_arm = crane_plus.get_planning_component('arm') - crane_plus_gripper = crane_plus.get_planning_component('gripper') + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='pick_and_place') logger.info('MoveItPy instance created') - # instantiate a RobotState instance using the current robot model + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm') + # グリッパ制御用 planning component + gripper = crane_plus.get_planning_component('gripper') + robot_model = crane_plus.get_robot_model() - robot_state = RobotState(robot_model) - # planningのパラメータ設定 - # armのパラメータ設定用 arm_plan_request_params = PlanRequestParameters( crane_plus, 'ompl_rrtc', ) - - # Set 0.0 ~ 1.0 - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 - - # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 1.0 - - # gripperのパラメータ設定用 gripper_plan_request_params = PlanRequestParameters( crane_plus, 'ompl_rrtc', ) - # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # gripperの開閉角度 + # グリッパの開閉角 GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - # armを現在の位置から'vertical'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='vertical') + # 物体を掴む位置 + rightward = R.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True) + quat = rightward.as_quat() + PRE_GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.17), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) + + downward = R.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True) + quat = downward.as_quat() + GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.14), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) + + # 物体を置く位置 + forward = R.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True) + quat = forward.as_quat() + RELEASE_POSE = Pose(position=Point(x=0.15, y=0.0, z=0.06), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # gripperを0度の位置に動かす + # gripperの開閉角を0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # ----- Picking Preparation ----- - # armを現在の位置から'home'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='home') + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # なにか掴んでいたときのためにgripperを開く + # gripperを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # armが掴みに行く位置を指定して動かす - pose_goal = PoseStamped() - pose_goal.header.frame_id = 'crane_plus_base' - - pose_goal.pose.position.x = 0.0 - pose_goal.pose.position.y = -0.09 - pose_goal.pose.position.z = 0.17 - q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(-90)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' - ) + # 物体の上に腕を伸ばす + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = PRE_GRASP_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # armの姿勢を変える(Y軸反転) - q = euler_to_quaternion(math.radians(0), math.radians(180), math.radians(-90)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' - ) + # 掴みに行く + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = GRASP_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # armをz軸上に動かす - pose_goal.pose.position.z = 0.14 - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' - ) - plan_and_execute( - crane_plus, - crane_plus_arm, - logger, - single_plan_parameters=arm_plan_request_params, - ) - - # Grasp + # ハンドを閉じる + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # armをz軸上に動かす - pose_goal.pose.position.z = 0.17 - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' - ) + # 持ち上げる + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = PRE_GRASP_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # ----- Placing Preparation ----- - # armを現在の位置から'home'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='home') + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # armが掴みに行く位置を指定して動かす - pose_goal.pose.position.x = 0.15 - pose_goal.pose.position.y = 0.0 - pose_goal.pose.position.z = 0.06 - q = euler_to_quaternion(math.radians(0), math.radians(90), math.radians(0)) - pose_goal.pose.orientation.x = q[0] - pose_goal.pose.orientation.y = q[1] - pose_goal.pose.orientation.z = q[2] - pose_goal.pose.orientation.w = q[3] - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state( - pose_stamped_msg=pose_goal, pose_link='crane_plus_link4' - ) + # 下ろす + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = RELEASE_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # Release + # ハンドを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # Return to home and vertical pose - # armを現在の位置から'home'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='home') + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # armを現在の位置から'vertical'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='vertical') + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=arm_plan_request_params, ) - # gripperの開閉をデフォルトの間隔に戻す + # gripperの開閉角を0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) - crane_plus_gripper.set_start_state_to_current_state() - crane_plus_gripper.set_goal_state(robot_state=robot_state) + gripper.set_goal_state(robot_state=robot_state) plan_and_execute( crane_plus, - crane_plus_gripper, + gripper, logger, single_plan_parameters=gripper_plan_request_params, ) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 9f890088..aa0321ee 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -11,13 +11,9 @@ import time -import numpy as np -import quaternion - from rclpy.logging import get_logger -# plan and execute def plan_and_execute( robot, planning_component, @@ -26,17 +22,11 @@ def plan_and_execute( multi_plan_parameters=None, sleep_time=0.0, ): - # plan to goal - logger = get_logger('plan_and_execute') logger.info('Planning trajectory') if multi_plan_parameters is not None: - plan_result = planning_component.plan( - multi_plan_parameters=multi_plan_parameters - ) + plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters) elif single_plan_parameters is not None: - plan_result = planning_component.plan( - single_plan_parameters=single_plan_parameters - ) + plan_result = planning_component.plan(single_plan_parameters=single_plan_parameters) else: plan_result = planning_component.plan() @@ -48,31 +38,5 @@ def plan_and_execute( result = robot.execute(robot_trajectory, controllers=[]) else: logger.error('Planning failed') - result = False - time.sleep(sleep_time) - return result - - -# euler --> quaternion -def euler_to_quaternion(roll, pitch, yaw): - cy = np.cos(yaw * 0.5) - sy = np.sin(yaw * 0.5) - cr = np.cos(roll * 0.5) - sr = np.sin(roll * 0.5) - cp = np.cos(pitch * 0.5) - sp = np.sin(pitch * 0.5) - qw = cy * cr * cp + sy * sr * sp - qx = cy * sr * cp - sy * cr * sp - qy = cy * cr * sp + sy * sr * cp - qz = sy * cr * cp - cy * sr * sp - - return [qx, qy, qz, qw] - - -# rotation matrix --> quaternion -def rotation_matrix_to_quaternion(rotation_matrix): - # numpy-quaternionを使用して回転行列からクォータニオンを計算 - # 3x3の回転行列をnumpy.quaternionに変換する - q = quaternion.from_rotation_matrix(rotation_matrix) - return q + time.sleep(sleep_time) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index 1abadb16..95cd6b46 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,8 +12,6 @@ # 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 crane_plus_description.robot_description_loader \ import RobotDescriptionLoader @@ -21,58 +19,24 @@ from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -# from launch_ros.actions import SetParameter from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): - # declare_loaded_description = DeclareLaunchArgument( - # "loaded_description', - # default_value='', - # description='Set robot_description text. \ - # It is recommended to use RobotDescriptionLoader() in \ - # crane_plus_description.', - # ) - ld = LaunchDescription() description_loader = RobotDescriptionLoader() - ld.add_action( - DeclareLaunchArgument( - 'loaded_description', - default_value=description_loader.load(), - description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() \ - in crane_plus_description.', - ) + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in crane_plus_description.', ) - moveit_config = ( - MoveItConfigsBuilder('crane_plus') + moveit_config = (MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, ) - .robot_description( - file_path=os.path.join( - get_package_share_directory('crane_plus_description'), - 'urdf', - 'crane_plus.urdf.xacro', - ), - mappings={}, - ) - .robot_description_kinematics( - file_path=get_package_share_directory('crane_plus_moveit_config') - + '/config/kinematics.yaml' - ) - # .robot_description_semantic( - # file_path='config/crsane_plus.srdf', - # mappings={'model': 'crane_plus'}, - # ) - # .joint_limits(file_path='config/joint_limits.yaml') - .trajectory_execution( - file_path=get_package_share_directory('crane_plus_moveit_config') - + '/config/controllers.yaml' - ) - # .planning_pipelines(pipelines=['ompl']) .moveit_cpp( file_path=get_package_share_directory('crane_plus_examples_py') + '/config/crane_plus_moveit_py_examples.yaml' @@ -84,42 +48,37 @@ def generate_launch_description(): 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {'capabilities': ''} - declare_example_name = DeclareLaunchArgument( 'example', default_value='gripper_control', - description=( - 'Set an example executable name: ' - '[gripper_control, pose_groupstate, \ - joint_values, pick_and_place, pick_and_place_tf]' - ), + description=('Set an example executable name: ' + '[gripper_control, pose_groupstate, \ + joint_values, pick_and_place]' + ) ) - # declare_use_sim_time = DeclareLaunchArgument( - # 'use_sim_time', - # default_value='false', - # description=('Set true when using the gazebo simulator.'), - # ) + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), + ) - # use_sim_time_name = SetParameter( - # name='use_sim_time', value=LaunchConfiguration('use_sim_time') - # ) + # 下記Issue対応のためここでパラメータを設定する + # https://github.com/moveit/moveit2/issues/2940#issuecomment-2401302214 + config_dict = moveit_config.to_dict() + config_dict.update({'use_sim_time': LaunchConfiguration('use_sim_time')}) example_node = Node( name=[LaunchConfiguration('example'), '_node'], package='crane_plus_examples_py', executable=LaunchConfiguration('example'), output='screen', - parameters=[moveit_config.to_dict()], + parameters=[config_dict], ) - # ld = LaunchDescription([SetParameter( - # name='use_sim_time', value=LaunchConfiguration('use_sim_time'))]) - # ld.add_action(declare_use_sim_time) - # ld.add_action(use_sim_time_name) - # ld.add_action(declare_loaded_description) - ld.add_action(declare_example_name) - ld.add_action(example_node) - - return ld + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + example_node + ]) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index bc22daec..e73cd183 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -14,8 +14,8 @@ std_msgs moveit moveit_py - python3-pip - python-numpy-quaternion-pip + python3-numpy + python3-scipy ament_copyright ament_flake8 From f2fc690a4d792c2977d683a60376232f01cd3322 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 17 Jan 2025 14:42:24 +0900 Subject: [PATCH 082/111] =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=87=E3=82=A3?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB=E3=82=92?= =?UTF-8?q?=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pose_groupstate.py | 46 +++++++++---------- 1 file changed, 21 insertions(+), 25 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index 03031e22..428bf700 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -14,71 +14,67 @@ from crane_plus_examples_py.utils import plan_and_execute -# moveit python library from moveit.planning import ( MoveItPy, PlanRequestParameters, ) + import rclpy from rclpy.logging import get_logger def main(args=None): - # ros2の初期化 rclpy.init(args=args) - - # ロガー生成 logger = get_logger('pose_groupstate') - # MoveItPy初期化 - crane_plus = MoveItPy(node_name='moveit_py') - crane_plus_arm = crane_plus.get_planning_component('arm') + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='pose_groupstate') logger.info('MoveItPy instance created') - # planningのパラメータ設定 + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm') + plan_request_params = PlanRequestParameters( crane_plus, 'ompl_rrtc', ) - # 速度＆加速度のスケーリングファクタを設定 + # 動作速度の調整 plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - # 現在の位置から'home'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='home') + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=plan_request_params, ) - # 現在の位置から'vertical'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='vertical') + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=plan_request_params, ) - # 現在の位置から'home'ポジションに動かす - crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name='home') + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') plan_and_execute( crane_plus, - crane_plus_arm, + arm, logger, single_plan_parameters=plan_request_params, ) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 rclpy.shutdown() From accc73267a68884d265770dc9bc33a376f717c6d Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 17 Jan 2025 18:02:32 +0900 Subject: [PATCH 083/111] =?UTF-8?q?=E5=8B=95=E4=BD=9C=E3=81=AE=E6=88=90?= =?UTF-8?q?=E5=90=A6=E3=82=92=E8=BF=94=E3=82=8A=E5=80=A4=E3=81=A8=E3=81=97?= =?UTF-8?q?=E3=81=A6=E5=87=BA=E5=8A=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/crane_plus_examples_py/utils.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index aa0321ee..c237413d 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -38,5 +38,7 @@ def plan_and_execute( result = robot.execute(robot_trajectory, controllers=[]) else: logger.error('Planning failed') + result = False time.sleep(sleep_time) + return result From d512a867eb208bcb5d5c4bb97af4a0fc0311a4c4 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Fri, 17 Jan 2025 18:39:17 +0900 Subject: [PATCH 084/111] =?UTF-8?q?capabilities=E3=81=AE=E8=A8=98=E8=BF=B0?= =?UTF-8?q?=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_moveit_config/launch/run_move_group.launch.py | 4 ---- 1 file changed, 4 deletions(-) mode change 100755 => 100644 crane_plus_moveit_config/launch/run_move_group.launch.py diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py old mode 100755 new mode 100644 index 0a8ec0b8..d8a43832 --- a/crane_plus_moveit_config/launch/run_move_group.launch.py +++ b/crane_plus_moveit_config/launch/run_move_group.launch.py @@ -53,10 +53,6 @@ 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 2f4d5fd73aa1604cdfc739ffea6d7f20e962d777 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Wed, 22 Jan 2025 13:24:59 +0900 Subject: [PATCH 085/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E5=87=A6?= =?UTF-8?q?=E7=90=86=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 1 - .../launch/camera_example.launch.py | 74 +++++++------------ 2 files changed, 27 insertions(+), 48 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index b3ffa174..34858175 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -from crane_plus_examples_py.utils import rotation_matrix_to_quaternion import cv2 from cv2 import aruco from cv_bridge import CvBridge diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index e92488d4..c077fe92 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -1,4 +1,4 @@ -# Copyright 2023 RT Corporation +# Copyright 2025 RT Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -12,8 +12,6 @@ # 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 crane_plus_description.robot_description_loader \ import RobotDescriptionLoader @@ -25,16 +23,13 @@ def generate_launch_description(): - ld = LaunchDescription() description_loader = RobotDescriptionLoader() - ld.add_action( - DeclareLaunchArgument( - 'loaded_description', - default_value=description_loader.load(), - description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() \ - in crane_plus_description.', - ) + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in crane_plus_description.', ) moveit_config = ( @@ -43,28 +38,6 @@ def generate_launch_description(): publish_robot_description=True, publish_robot_description_semantic=True, ) - .robot_description( - file_path=os.path.join( - get_package_share_directory('crane_plus_description'), - 'urdf', - 'crane_plus.urdf.xacro', - ), - mappings={}, - ) - .robot_description_kinematics( - file_path=get_package_share_directory('crane_plus_moveit_config') - + '/config/kinematics.yaml' - ) - # .robot_description_semantic( - # file_path='config/crsane_plus.srdf', - # mappings={'model': 'crane_plus'}, - # ) - # .joint_limits(file_path='config/joint_limits.yaml') - .trajectory_execution( - file_path=get_package_share_directory('crane_plus_moveit_config') - + '/config/controllers.yaml' - ) - # .planning_pipelines(pipelines=['ompl']) .moveit_cpp( file_path=get_package_share_directory('crane_plus_examples_py') + '/config/crane_plus_moveit_py_examples.yaml' @@ -84,17 +57,23 @@ def generate_launch_description(): '[aruco_detection, color_detection]') ) + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), + ) + + # 下記Issue対応のためここでパラメータを設定する + # https://github.com/moveit/moveit2/issues/2940#issuecomment-2401302214 + config_dict = moveit_config.to_dict() + config_dict.update({'use_sim_time': LaunchConfiguration('use_sim_time')}) + picking_node = Node( name='pick_and_place_tf', package='crane_plus_examples_py', executable='pick_and_place_tf', output='screen', - parameters=[ - # moveit_config.robot_description, - # moveit_config.robot_description_semantic, - # moveit_config.robot_description_kinematics, - moveit_config.to_dict() - ], + parameters=[config_dict] ) example_node = Node( @@ -102,12 +81,13 @@ def generate_launch_description(): package='crane_plus_examples_py', executable=LaunchConfiguration('example'), output='screen', - parameters=[moveit_config.to_dict()], + parameters=[config_dict], ) - # ld = LaunchDescription() - ld.add_action(example_node) - ld.add_action(picking_node) - ld.add_action(declare_example_name) - - return ld + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + picking_node, + example_node + ]) From 44dc4a4394d1f05c4c019a0108e7ca8d72387da1 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Wed, 22 Jan 2025 13:26:21 +0900 Subject: [PATCH 086/111] =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=87=E3=82=A3?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E7=B5=B1=E4=B8=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- crane_plus_examples_py/launch/camera_example.launch.py | 2 -- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 25b87d32..eee4c465 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -15,7 +15,7 @@ import datetime import math -from crane_plus_examples_py.utils import euler_to_quaternion, plan_and_execute +from crane_plus_examples_py.utils import plan_and_execute from geometry_msgs.msg import PoseStamped diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index c077fe92..35ea2af1 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -49,8 +49,6 @@ def generate_launch_description(): 'robot_description': LaunchConfiguration('loaded_description') } - moveit_config.move_group_capabilities = {'capabilities': ''} - declare_example_name = DeclareLaunchArgument( 'example', default_value='color_detection', description=('Set an example executable name: ' From 02070cf712eb7339c49556033a3c7915d123c312 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Wed, 22 Jan 2025 17:34:37 +0900 Subject: [PATCH 087/111] =?UTF-8?q?=E5=9B=9E=E8=BB=A2=E8=A1=8C=E5=88=97?= =?UTF-8?q?=E3=81=AE=E5=A4=89=E6=8F=9B=E3=81=ABscipy=E3=82=92=E4=BD=BF?= =?UTF-8?q?=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 28 ++++++++----------- 1 file changed, 11 insertions(+), 17 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 34858175..d3018372 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -19,6 +19,7 @@ import numpy as np import rclpy from rclpy.node import Node +from scipy.spatial.transform import Rotation from sensor_msgs.msg import CameraInfo, Image import tf2_ros @@ -35,34 +36,28 @@ def __init__(self): self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None - self.bridge = CvBridge() def image_callback(self, msg): # 画像データをROSのメッセージからOpenCVの配列に変換 - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + bridge = CvBridge() + cv_img = bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) if self.camera_info: # ArUcoマーカのデータセットを読み込む # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) - - # マーカーID + # マーカID ids = [] - # 画像座標系上のマーカ頂点位置 corners = [] - # マーカの検出 corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) - # マーカの検出数 n_markers = len(ids) - # カメラパラメータ CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) - # マーカ一辺の長さ 0.04 [m] MARKER_LENGTH = 0.04 @@ -79,14 +74,13 @@ def image_callback(self, msg): t.transform.translation.x = tvecs[i][0][0] t.transform.translation.y = tvecs[i][0][1] t.transform.translation.z = tvecs[i][0][2] - # 回転ベクトル→回転行列 - rotation_matrix, _ = cv2.Rodrigues(rvecs[i]) - # 回転行列からクォータニオンを求める - q = rotation_matrix_to_quaternion(rotation_matrix) - t.transform.rotation.x = q.x - t.transform.rotation.y = q.y - t.transform.rotation.z = q.z - t.transform.rotation.w = q.w + # 回転ベクトルをクォータニオンに変換 + marker_orientation_rot = Rotation.from_rotvec(rvecs) + marker_orientation_quat = marker_orientation_rot.as_quat() + t.transform.rotation.x = marker_orientation_quat[0] + t.transform.rotation.y = marker_orientation_quat[1] + t.transform.rotation.z = marker_orientation_quat[2] + t.transform.rotation.w = marker_orientation_quat[3] self.tf_broadcaster.sendTransform(t) def camera_info_callback(self, msg): From ab329fa4f6158accf357a489e5bd939f8df47eb0 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Wed, 22 Jan 2025 18:35:37 +0900 Subject: [PATCH 088/111] =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=87=E3=82=A3?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=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 --- .../crane_plus_examples_py/aruco_detection.py | 4 +- .../crane_plus_examples_py/color_detection.py | 39 ++++++++----------- .../crane_plus_examples_py/pick_and_place.py | 8 ++-- .../launch/example.launch.py | 10 ++--- 4 files changed, 28 insertions(+), 33 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index d3018372..ec4ead83 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -36,11 +36,11 @@ def __init__(self): self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None + self.bridge = CvBridge() def image_callback(self, msg): # 画像データをROSのメッセージからOpenCVの配列に変換 - bridge = CvBridge() - cv_img = bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) if self.camera_info: diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index 08c39601..d2fa0c14 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -33,7 +33,7 @@ def __init__(self): ) self.image_thresholded_publisher = self.create_publisher( Image, 'image_thresholded', 10 - ) + ) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None self.bridge = CvBridge() @@ -43,18 +43,16 @@ def image_callback(self, msg): # 赤い物体を検出するようにHSVの範囲を設定 # 周囲の明るさ等の動作環境に合わせて調整 LOW_H_1 = 0 - HIGH_H_1 = 20 - LOW_H_2 = 160 + HIGH_H_1 = 10 + LOW_H_2 = 170 HIGH_H_2 = 179 LOW_S = 100 HIGH_S = 255 - LOW_V = 50 + LOW_V = 100 HIGH_V = 255 # ウェブカメラの画像を受け取る - cv_img = self.bridge.imgmsg_to_cv2( - msg, desired_encoding=msg.encoding - ) + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) @@ -62,10 +60,10 @@ def image_callback(self, msg): # 画像の二値化 img_mask_1 = cv2.inRange( cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V) - ) + ) img_mask_2 = cv2.inRange( cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V) - ) + ) # マスク画像の合成 img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) @@ -74,13 +72,12 @@ def image_callback(self, msg): kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) img_thresholded = cv2.morphologyEx( img_thresholded, cv2.MORPH_OPEN, kernel - ) + ) # 穴埋めの処理 - kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) img_thresholded = cv2.morphologyEx( img_thresholded, cv2.MORPH_CLOSE, kernel - ) + ) # 画像の検出領域におけるモーメントを計算 moment = cv2.moments(img_thresholded) @@ -89,7 +86,7 @@ def image_callback(self, msg): d_area = moment['m00'] # 検出した領域のピクセル数が10000より大きい場合 - if d_area < 10000: + if d_area > 10000: # カメラモデル作成 camera_model = PinholeCameraModel() @@ -109,25 +106,23 @@ def image_callback(self, msg): # カメラの高さを0.44[m]として把持対象物の位置を計算 CAMERA_HEIGHT = 0.46 - object_position = { - 'x': ray.x * CAMERA_HEIGHT, - 'y': ray.y * CAMERA_HEIGHT, - 'z': ray.z * CAMERA_HEIGHT, - } + object_position = [ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT] # 把持対象物の位置をTFに配信 t = TransformStamped() t.header = msg.header t.child_frame_id = 'target_0' - t.transform.translation.x = object_position['x'] - t.transform.translation.y = object_position['y'] - t.transform.translation.z = object_position['z'] + t.transform.translation.x = object_position[0] + t.transform.translation.y = object_position[1] + t.transform.translation.z = object_position[2] self.tf_broadcaster.sendTransform(t) # 閾値による二値化画像を配信 img_thresholded_msg = self.bridge.cv2_to_imgmsg( img_thresholded, encoding='mono8' - ) + ) self.image_thresholded_publisher.publish(img_thresholded_msg) def camera_info_callback(self, msg): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 2094dcde..1439ef26 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -24,7 +24,7 @@ import rclpy from rclpy.logging import get_logger -from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Rotation def main(args=None): @@ -64,18 +64,18 @@ def main(args=None): GRIPPER_CLOSE = math.radians(10.0) # 物体を掴む位置 - rightward = R.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True) + rightward = Rotation.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True) quat = rightward.as_quat() PRE_GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.17), orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) - downward = R.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True) + downward = Rotation.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True) quat = downward.as_quat() GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.14), orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) # 物体を置く位置 - forward = R.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True) + forward = Rotation.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True) quat = forward.as_quat() RELEASE_POSE = Pose(position=Point(x=0.15, y=0.0, z=0.06), orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index 95cd6b46..3915f40f 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -32,7 +32,8 @@ def generate_launch_description(): in crane_plus_description.', ) - moveit_config = (MoveItConfigsBuilder('crane_plus') + moveit_config = ( + MoveItConfigsBuilder('crane_plus') .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True, @@ -51,10 +52,9 @@ def generate_launch_description(): declare_example_name = DeclareLaunchArgument( 'example', default_value='gripper_control', - description=('Set an example executable name: ' - '[gripper_control, pose_groupstate, \ - joint_values, pick_and_place]' - ) + description='Set an example executable name: \ + [gripper_control, pose_groupstate, \ + joint_values, pick_and_place]' ) declare_use_sim_time = DeclareLaunchArgument( From 314302ebd67ff263e768bca995305253d6a52631 Mon Sep 17 00:00:00 2001 From: Atsushi Kuwagata Date: Wed, 22 Jan 2025 18:45:01 +0900 Subject: [PATCH 089/111] =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=87=E3=82=A3?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E3=82=B9=E3=82=BF=E3=82=A4=E3=83=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 --- .../crane_plus_examples_py/pick_and_place_tf.py | 13 +++++++------ .../crane_plus_examples_py/utils.py | 2 -- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index eee4c465..bc93c5e1 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -36,6 +36,7 @@ import tf2_ros from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer +from scipy.spatial.transform import Rotation class PickAndPlaceTf(Node): @@ -223,12 +224,12 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose.pose.position.x = x target_pose.pose.position.y = y target_pose.pose.position.z = z - q = euler_to_quaternion(math.radians(roll), math.radians(pitch), - math.radians(yaw)) - target_pose.pose.orientation.x = q[0] - target_pose.pose.orientation.y = q[1] - target_pose.pose.orientation.z = q[2] - target_pose.pose.orientation.w = q[3] + rotation = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True) + quat = rotation.as_quat() + target_pose.pose.orientation.x = quat[0] + target_pose.pose.orientation.y = quat[1] + target_pose.pose.orientation.z = quat[2] + target_pose.pose.orientation.w = quat[3] self.crane_plus_arm.set_start_state_to_current_state() self.crane_plus_arm.set_goal_state( pose_stamped_msg=target_pose, pose_link='crane_plus_link4' diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index c237413d..8e363cee 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -11,8 +11,6 @@ import time -from rclpy.logging import get_logger - def plan_and_execute( robot, From 4fb8ab6a801f6c0d602e065f3aa7d6ad559315f3 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 13:51:52 +0900 Subject: [PATCH 090/111] =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E5=A7=BF=E5=8B=A2?= =?UTF-8?q?=E3=81=AE=E8=A8=B1=E5=AE=B9=E7=AF=84=E5=9B=B2=E3=82=92=E8=A8=AD?= =?UTF-8?q?=E5=AE=9A=E3=81=97=E3=81=A6=E3=83=97=E3=83=A9=E3=83=B3=E3=83=8B?= =?UTF-8?q?=E3=83=B3=E3=82=B0=E3=82=92=E5=AE=9F=E8=A1=8C=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place.py | 92 +++++++++++++------ 1 file changed, 62 insertions(+), 30 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 1439ef26..0d5e01fb 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -17,6 +17,8 @@ from crane_plus_examples_py.utils import plan_and_execute from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion from moveit.core.robot_state import RobotState +from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume +from shape_msgs.msg import SolidPrimitive from moveit.planning import ( MoveItPy, PlanRequestParameters, @@ -27,6 +29,58 @@ from scipy.spatial.transform import Rotation +def set_goal_constraints(x, y, z, roll, pitch, yaw): + # 位置姿勢の許容誤差 + POSITION_TOLERANCE = 0.00001 + ORIENTATION_TOLERANCE = 0.0001 + + # 目標位置姿勢 + target_pose = PoseStamped() + target_pose.header.frame_id = 'crane_plus_base' + target_pose.pose.position.x = x + target_pose.pose.position.y = y + target_pose.pose.position.z = z + rotation = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True) + quat = rotation.as_quat() + target_pose.pose.orientation.x = quat[0] + target_pose.pose.orientation.y = quat[1] + target_pose.pose.orientation.z = quat[2] + target_pose.pose.orientation.w = quat[3] + + # 目標位置姿勢の制約設定 + goal_constraints = Constraints() + goal_constraints.name = "tolerance_goal" + + # 位置の制約設定 + position_constraint = PositionConstraint() + position_constraint.header.frame_id = 'crane_plus_base' + position_constraint.link_name = 'crane_plus_link4' + tolerance_region = BoundingVolume() + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.SPHERE + primitive.dimensions = [POSITION_TOLERANCE] + tolerance_region.primitives.append(primitive) + tolerance_region.primitive_poses.append(target_pose.pose) + position_constraint.constraint_region = tolerance_region + position_constraint.weight = 1.0 + + # 姿勢の制約設定 + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = 'crane_plus_base' + orientation_constraint.link_name = 'crane_plus_link4' + orientation_constraint.orientation = target_pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_z_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.weight = 1.0 + + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orientation_constraint) + + # 制約設定を返す + return goal_constraints + + def main(args=None): rclpy.init(args=args) logger = get_logger('pick_and_place') @@ -63,22 +117,8 @@ def main(args=None): GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - # 物体を掴む位置 - rightward = Rotation.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True) - quat = rightward.as_quat() - PRE_GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.17), - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) - - downward = Rotation.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True) - quat = downward.as_quat() - GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.14), - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) # 物体を置く位置 - forward = Rotation.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True) - quat = forward.as_quat() - RELEASE_POSE = Pose(position=Point(x=0.15, y=0.0, z=0.06), - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) # SRDF内に定義されている'vertical'の姿勢にする arm.set_start_state_to_current_state() @@ -126,10 +166,8 @@ def main(args=None): # 物体の上に腕を伸ばす arm.set_start_state_to_current_state() - goal_pose = PoseStamped() - goal_pose.header.frame_id = 'crane_plus_base' - goal_pose.pose = PRE_GRASP_POSE - arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + goal_constraints = set_goal_constraints(0.0, -0.09, 0.17, 0.0, 90.0, -90.0) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, arm, @@ -139,10 +177,8 @@ def main(args=None): # 掴みに行く arm.set_start_state_to_current_state() - goal_pose = PoseStamped() - goal_pose.header.frame_id = 'crane_plus_base' - goal_pose.pose = GRASP_POSE - arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + goal_constraints = set_goal_constraints(0.0, -0.09, 0.14, 0.0, 180.0, -90.0) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, arm, @@ -164,10 +200,8 @@ def main(args=None): # 持ち上げる arm.set_start_state_to_current_state() - goal_pose = PoseStamped() - goal_pose.header.frame_id = 'crane_plus_base' - goal_pose.pose = PRE_GRASP_POSE - arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + goal_constraints = set_goal_constraints(0.0, -0.09, 0.17, 0.0, 90.0, -90.0) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, arm, @@ -187,10 +221,8 @@ def main(args=None): # 下ろす arm.set_start_state_to_current_state() - goal_pose = PoseStamped() - goal_pose.header.frame_id = 'crane_plus_base' - goal_pose.pose = RELEASE_POSE - arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + goal_constraints = set_goal_constraints(0.15, 0.0, 0.06, 0.0, 90.0, 0.0) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, arm, From f68e5cf8510428212a73df9cb9c15ee51a9d0650 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 14:57:22 +0900 Subject: [PATCH 091/111] =?UTF-8?q?=E8=87=AA=E5=8B=95=E3=83=95=E3=82=A9?= =?UTF-8?q?=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_control/crane_plus_driver.hpp | 5 +- .../crane_plus_hardware.hpp | 6 +- crane_plus_control/src/crane_plus_driver.cpp | 64 +++++-------- .../src/crane_plus_hardware.cpp | 96 ++++++------------- .../robot_description_loader.py | 32 ++++--- crane_plus_examples/src/aruco_detection.cpp | 30 +++--- crane_plus_examples/src/color_detection.cpp | 40 +++----- crane_plus_examples/src/gripper_control.cpp | 9 +- crane_plus_examples/src/joint_values.cpp | 9 +- crane_plus_examples/src/pick_and_place.cpp | 11 +-- crane_plus_examples/src/pick_and_place_tf.cpp | 35 +++---- crane_plus_examples/src/pose_groupstate.cpp | 4 +- .../crane_plus_examples_py/aruco_detection.py | 3 +- .../crane_plus_examples_py/color_detection.py | 32 +++---- .../crane_plus_examples_py/joint_values.py | 26 ++--- .../crane_plus_examples_py/pick_and_place.py | 11 +-- .../pick_and_place_tf.py | 24 ++--- .../launch/camera_example.launch.py | 27 +++--- .../launch/example.launch.py | 14 +-- crane_plus_examples_py/setup.py | 12 +-- crane_plus_examples_py/test/test_flake8.py | 4 +- 21 files changed, 189 insertions(+), 305 deletions(-) diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp index bae3c1e8..71df449e 100644 --- a/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp +++ b/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp @@ -16,6 +16,7 @@ #define CRANE_PLUS_CONTROL__CRANE_PLUS_DRIVER_HPP_ #include + #include #include #include @@ -50,8 +51,8 @@ class CranePlusDriver bool read_byte_data_from_each_joints(const uint16_t address, std::vector & buffer); bool read_word_data_from_each_joints(const uint16_t address, std::vector & buffer); bool parse_dxl_error( - const std::string func_name, const uint8_t dxl_id, - const int dxl_comm_result, const uint8_t dxl_packet_error); + const std::string func_name, const uint8_t dxl_id, const int dxl_comm_result, + const uint8_t dxl_packet_error); double dxl_pos_to_radian(const uint16_t position); uint16_t radian_to_dxl_pos(const double position); double dxl_speed_to_rps(const uint16_t speed); diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp index 60309cda..64fe66e7 100644 --- a/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp +++ b/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ #define CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ @@ -31,13 +30,12 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -using hardware_interface::return_type; using hardware_interface::CallbackReturn; +using hardware_interface::return_type; namespace crane_plus_control { -class CranePlusHardware : public - hardware_interface::SystemInterface +class CranePlusHardware : public hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(CranePlusHardware) diff --git a/crane_plus_control/src/crane_plus_driver.cpp b/crane_plus_control/src/crane_plus_driver.cpp index 6d3de2e7..acc335cf 100644 --- a/crane_plus_control/src/crane_plus_driver.cpp +++ b/crane_plus_control/src/crane_plus_driver.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "crane_plus_control/crane_plus_driver.hpp" + #include #include #include #include #include -#include "crane_plus_control/crane_plus_driver.hpp" - constexpr double PROTOCOL_VERSION = 1.0; constexpr int DXL_HOME_POSITION = 511; // value range:0 ~ 1023 constexpr double DXL_MAX_POSITION = 1023.0; @@ -44,8 +44,7 @@ constexpr uint16_t ADDR_PRESENT_VOLTAGE = 42; constexpr uint16_t ADDR_PRESENT_TEMPERATURE = 43; CranePlusDriver::CranePlusDriver( - const std::string port_name, const int baudrate, - std::vector id_list) + const std::string port_name, const int baudrate, std::vector id_list) : baudrate_(baudrate), id_list_(id_list) { dxl_port_handler_ = std::shared_ptr( @@ -54,37 +53,28 @@ CranePlusDriver::CranePlusDriver( dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION)); } -CranePlusDriver::~CranePlusDriver() -{ - close_port(); -} +CranePlusDriver::~CranePlusDriver() { close_port(); } bool CranePlusDriver::open_port(void) { if (!dxl_port_handler_->openPort()) { - last_error_log_ = std::string(__func__) + ": unable to open dynamixel port: " + - dxl_port_handler_->getPortName(); + last_error_log_ = std::string(__func__) + + ": unable to open dynamixel port: " + dxl_port_handler_->getPortName(); return false; } if (!dxl_port_handler_->setBaudRate(baudrate_)) { last_error_log_ = std::string(__func__) + ": unable to set baudrate" + - std::to_string(dxl_port_handler_->getBaudRate()); + std::to_string(dxl_port_handler_->getBaudRate()); return false; } return true; } -void CranePlusDriver::close_port(void) -{ - dxl_port_handler_->closePort(); -} +void CranePlusDriver::close_port(void) { dxl_port_handler_->closePort(); } -std::string CranePlusDriver::get_last_error_log(void) -{ - return last_error_log_; -} +std::string CranePlusDriver::get_last_error_log(void) { return last_error_log_; } bool CranePlusDriver::torque_enable(const bool enable) { @@ -92,8 +82,7 @@ bool CranePlusDriver::torque_enable(const bool enable) for (auto dxl_id : id_list_) { uint8_t dxl_error = 0; int dxl_result = dxl_packet_handler_->write1ByteTxRx( - dxl_port_handler_.get(), - dxl_id, ADDR_TORQUE_ENABLE, enable, &dxl_error); + dxl_port_handler_.get(), dxl_id, ADDR_TORQUE_ENABLE, enable, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -107,8 +96,8 @@ bool CranePlusDriver::write_goal_joint_positions(const std::vector & goa { if (goal_positions.size() != id_list_.size()) { last_error_log_ = std::string(__func__) + ": vectors size does not match: " + - " goal_positions:" + std::to_string(goal_positions.size()) + - ", id_list:" + std::to_string(id_list_.size()); + " goal_positions:" + std::to_string(goal_positions.size()) + + ", id_list:" + std::to_string(id_list_.size()); return false; } @@ -119,8 +108,7 @@ bool CranePlusDriver::write_goal_joint_positions(const std::vector & goa uint16_t goal_position = radian_to_dxl_pos(goal_positions[i]); auto dxl_id = id_list_[i]; int dxl_result = dxl_packet_handler_->write2ByteTxRx( - dxl_port_handler_.get(), - dxl_id, ADDR_GOAL_POSITION, goal_position, &dxl_error); + dxl_port_handler_.get(), dxl_id, ADDR_GOAL_POSITION, goal_position, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -135,8 +123,7 @@ bool CranePlusDriver::write_moving_speed_rpm(const uint8_t dxl_id, const double const int DXL_MAX_MOVING_SPEED = 1023; const double SPEED_UNIT = 0.111; // rpm if (std::find(id_list_.begin(), id_list_.end(), dxl_id) == id_list_.end()) { - last_error_log_ = std::string(__func__) + ": dxl_id: " + std::to_string(dxl_id) + - "not found."; + last_error_log_ = std::string(__func__) + ": dxl_id: " + std::to_string(dxl_id) + "not found."; return false; } @@ -153,8 +140,7 @@ bool CranePlusDriver::write_moving_speed_rpm(const uint8_t dxl_id, const double uint8_t dxl_error = 0; int dxl_result = dxl_packet_handler_->write2ByteTxRx( - dxl_port_handler_.get(), - dxl_id, ADDR_MOVING_SPEED, dxl_moving_speed, &dxl_error); + dxl_port_handler_.get(), dxl_id, ADDR_MOVING_SPEED, dxl_moving_speed, &dxl_error); retval = parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error); @@ -235,16 +221,14 @@ bool CranePlusDriver::read_present_joint_temperatures(std::vector & join } bool CranePlusDriver::read_byte_data_from_each_joints( - const uint16_t address, - std::vector & buffer) + const uint16_t address, std::vector & buffer) { bool retval = true; for (auto dxl_id : id_list_) { uint8_t dxl_error = 0; uint8_t data = 0; int dxl_result = dxl_packet_handler_->read1ByteTxRx( - dxl_port_handler_.get(), - dxl_id, address, &data, &dxl_error); + dxl_port_handler_.get(), dxl_id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -257,16 +241,14 @@ bool CranePlusDriver::read_byte_data_from_each_joints( } bool CranePlusDriver::read_word_data_from_each_joints( - const uint16_t address, - std::vector & buffer) + const uint16_t address, std::vector & buffer) { bool retval = true; for (auto dxl_id : id_list_) { uint8_t dxl_error = 0; uint16_t data = 0; int dxl_result = dxl_packet_handler_->read2ByteTxRx( - dxl_port_handler_.get(), - dxl_id, address, &data, &dxl_error); + dxl_port_handler_.get(), dxl_id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -279,20 +261,20 @@ bool CranePlusDriver::read_word_data_from_each_joints( } bool CranePlusDriver::parse_dxl_error( - const std::string func_name, const uint8_t dxl_id, - const int dxl_comm_result, const uint8_t dxl_packet_error) + const std::string func_name, const uint8_t dxl_id, const int dxl_comm_result, + const uint8_t dxl_packet_error) { bool retval = true; if (dxl_comm_result != COMM_SUCCESS) { last_error_log_ = func_name + ": dxl_id: " + std::to_string(dxl_id) + " :" + - std::string(dxl_packet_handler_->getTxRxResult(dxl_comm_result)); + std::string(dxl_packet_handler_->getTxRxResult(dxl_comm_result)); retval = false; } if (dxl_packet_error != 0) { last_error_log_ = func_name + ": dxl_id: " + std::to_string(dxl_id) + " :" + - std::string(dxl_packet_handler_->getRxPacketError(dxl_packet_error)); + std::string(dxl_packet_handler_->getRxPacketError(dxl_packet_error)); retval = false; } diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index 6514e000..3857d6a7 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -12,16 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "crane_plus_control/crane_plus_hardware.hpp" #include #include #include #include -#include "crane_plus_control/crane_plus_hardware.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" - namespace crane_plus_control { @@ -31,8 +30,7 @@ CranePlusHardware::~CranePlusHardware() driver_->close_port(); } -CallbackReturn CranePlusHardware::on_init( - const hardware_interface::HardwareInfo & info) +CallbackReturn CranePlusHardware::on_init(const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -54,8 +52,7 @@ CallbackReturn CranePlusHardware::on_init( dxl_id_list.push_back(std::stoi(joint.parameters["dxl_id"])); } else { RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), - "Joint '%s' does not have 'dxl_id' parameter.", + rclcpp::get_logger("CranePlusHardware"), "Joint '%s' does not have 'dxl_id' parameter.", joint.name.c_str()); return CallbackReturn::ERROR; } @@ -71,13 +68,11 @@ CallbackReturn CranePlusHardware::on_init( // Open a crane_plus_driver driver_ = std::make_shared(port_name, baudrate, dxl_id_list); if (!driver_->open_port()) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); return CallbackReturn::ERROR; } if (!driver_->torque_enable(false)) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); return CallbackReturn::ERROR; } @@ -86,74 +81,52 @@ CallbackReturn CranePlusHardware::on_init( if (joint.command_interfaces.size() != 1) { RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), - "Joint '%s' has %lu command interfaces found. 1 expected.", - joint.name.c_str(), joint.command_interfaces.size()); + "Joint '%s' has %lu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); return CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", - joint.name.c_str(), joint.command_interfaces[0].name.c_str(), - hardware_interface::HW_IF_POSITION); + "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return CallbackReturn::ERROR; } } steady_clock_ = rclcpp::Clock(RCL_STEADY_TIME); - return CallbackReturn::SUCCESS; } -std::vector -CranePlusHardware::export_state_interfaces() +std::vector CranePlusHardware::export_state_interfaces() { std::vector state_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_position_states_[i]) - ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_states_[i])); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, - &hw_velocity_states_[i]) - ); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_states_[i])); state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, "load", - &hw_load_states_[i]) - ); - state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, "voltage", - &hw_voltage_states_[i]) - ); + hardware_interface::StateInterface(info_.joints[i].name, "load", &hw_load_states_[i])); state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[i].name, "temperature", - &hw_temperature_states_[i]) - ); + hardware_interface::StateInterface(info_.joints[i].name, "voltage", &hw_voltage_states_[i])); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "temperature", &hw_temperature_states_[i])); } return state_interfaces; } -std::vector -CranePlusHardware::export_command_interfaces() +std::vector CranePlusHardware::export_command_interfaces() { std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, - &hw_position_commands_[i]) - ); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_commands_[i])); } return command_interfaces; @@ -162,9 +135,7 @@ CranePlusHardware::export_command_interfaces() CallbackReturn CranePlusHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { if (!driver_->torque_enable(false)) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), - driver_->get_last_error_log().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); return CallbackReturn::ERROR; } // Set current timestamp to disable the communication timeout. @@ -180,8 +151,7 @@ CallbackReturn CranePlusHardware::on_activate(const rclcpp_lifecycle::State & /* return CallbackReturn::SUCCESS; } -CallbackReturn CranePlusHardware::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn CranePlusHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { driver_->torque_enable(false); @@ -193,8 +163,7 @@ return_type CranePlusHardware::read( { if (communication_timeout()) { if (!timeout_has_printed_) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); timeout_has_printed_ = true; } return return_type::ERROR; @@ -202,9 +171,7 @@ return_type CranePlusHardware::read( std::vector joint_positions; if (!driver_->read_present_joint_positions(joint_positions)) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), - driver_->get_last_error_log().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); // readに失敗しても通信は継続させる。 // 不確かなデータをセットしないようにOKを返す。 return return_type::OK; @@ -243,7 +210,6 @@ return_type CranePlusHardware::read( } } - if (read_temperatures_) { std::vector joint_temperatures; if (driver_->read_present_joint_temperatures(joint_temperatures)) { @@ -262,17 +228,14 @@ return_type CranePlusHardware::write( { if (communication_timeout()) { if (!timeout_has_printed_) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); timeout_has_printed_ = true; } return return_type::ERROR; } if (!driver_->write_goal_joint_positions(hw_position_commands_)) { - RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), - driver_->get_last_error_log().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); // writeに失敗しても通信は継続させる。 // 不確かなデータをセットしないようにOKを返す。 return return_type::OK; @@ -295,7 +258,4 @@ bool CranePlusHardware::communication_timeout() #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - crane_plus_control::CranePlusHardware, - hardware_interface::SystemInterface -) +PLUGINLIB_EXPORT_CLASS(crane_plus_control::CranePlusHardware, hardware_interface::SystemInterface) diff --git a/crane_plus_description/crane_plus_description/robot_description_loader.py b/crane_plus_description/crane_plus_description/robot_description_loader.py index d62ac45f..de543a37 100755 --- a/crane_plus_description/crane_plus_description/robot_description_loader.py +++ b/crane_plus_description/crane_plus_description/robot_description_loader.py @@ -18,13 +18,11 @@ from launch.substitutions import Command -class RobotDescriptionLoader(): - +class RobotDescriptionLoader: def __init__(self): self.robot_description_path = os.path.join( - get_package_share_directory('crane_plus_description'), - 'urdf', - 'crane_plus.urdf.xacro') + get_package_share_directory('crane_plus_description'), 'urdf', 'crane_plus.urdf.xacro' + ) self.port_name = '/dev/ttyUSB0' self.use_gazebo = 'false' self.use_camera = 'false' @@ -33,13 +31,21 @@ def __init__(self): self.gz_control_config_file_path = '' def load(self): - return Command([ + return Command( + [ 'xacro ', self.robot_description_path, - ' port_name:=', self.port_name, - ' use_gazebo:=', self.use_gazebo, - ' use_camera:=', self.use_camera, - ' use_mock_components:=', self.use_mock_components, - ' gz_control_config_package:=', self.gz_control_config_package, - ' gz_control_config_file_path:=', self.gz_control_config_file_path - ]) + ' port_name:=', + self.port_name, + ' use_gazebo:=', + self.use_gazebo, + ' use_camera:=', + self.use_camera, + ' use_mock_components:=', + self.use_mock_components, + ' gz_control_config_package:=', + self.gz_control_config_package, + ' gz_control_config_file_path:=', + self.gz_control_config_file_path, + ] + ) diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index 77ca11d3..a6f26ba6 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -18,26 +18,25 @@ #include #include -#include #include +#include -#include "rclcpp/rclcpp.hpp" +#include "cv_bridge/cv_bridge.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" +#include "opencv2/aruco.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" -#include "opencv2/opencv.hpp" -#include "opencv2/aruco.hpp" -#include "cv_bridge/cv_bridge.hpp" -#include "tf2/LinearMath/Quaternion.hpp" #include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" #include "tf2_ros/transform_broadcaster.h" using std::placeholders::_1; class ImageSubscriber : public rclcpp::Node { public: - ImageSubscriber() - : Node("aruco_detection") + ImageSubscriber() : Node("aruco_detection") { image_subscription_ = this->create_subscription( "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); @@ -45,8 +44,7 @@ class ImageSubscriber : public rclcpp::Node camera_info_subscription_ = this->create_subscription( "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); - tf_broadcaster_ = - std::make_unique(*this); + tf_broadcaster_ = std::make_unique(*this); } private: @@ -98,14 +96,10 @@ class ImageSubscriber : public rclcpp::Node cv::Mat cv_rotation_matrix; cv::Rodrigues(rvecs[i], cv_rotation_matrix); tf2::Matrix3x3 tf2_rotation_matrix = tf2::Matrix3x3( - cv_rotation_matrix.at(0, 0), - cv_rotation_matrix.at(0, 1), - cv_rotation_matrix.at(0, 2), - cv_rotation_matrix.at(1, 0), - cv_rotation_matrix.at(1, 1), - cv_rotation_matrix.at(1, 2), - cv_rotation_matrix.at(2, 0), - cv_rotation_matrix.at(2, 1), + cv_rotation_matrix.at(0, 0), cv_rotation_matrix.at(0, 1), + cv_rotation_matrix.at(0, 2), cv_rotation_matrix.at(1, 0), + cv_rotation_matrix.at(1, 1), cv_rotation_matrix.at(1, 2), + cv_rotation_matrix.at(2, 0), cv_rotation_matrix.at(2, 1), cv_rotation_matrix.at(2, 2)); tf2_rotation_matrix.getRotation(q); t.transform.rotation.x = q.x(); diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp index 33e73894..c5e2a878 100644 --- a/crane_plus_examples/src/color_detection.cpp +++ b/crane_plus_examples/src/color_detection.cpp @@ -16,28 +16,27 @@ // https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html #include -#include #include +#include #include -#include "rclcpp/rclcpp.hpp" +#include "cv_bridge/cv_bridge.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" +#include "image_geometry/pinhole_camera_model.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" -#include "tf2/LinearMath/Quaternion.hpp" #include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2/LinearMath/Quaternion.hpp" #include "tf2_ros/transform_broadcaster.h" -#include "opencv2/opencv.hpp" -#include "opencv2/imgproc/imgproc.hpp" -#include "cv_bridge/cv_bridge.hpp" -#include "image_geometry/pinhole_camera_model.hpp" using std::placeholders::_1; class ImageSubscriber : public rclcpp::Node { public: - ImageSubscriber() - : Node("color_detection") + ImageSubscriber() : Node("color_detection") { image_subscription_ = this->create_subscription( "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); @@ -48,8 +47,7 @@ class ImageSubscriber : public rclcpp::Node image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); - tf_broadcaster_ = - std::make_unique(*this); + tf_broadcaster_ = std::make_unique(*this); } private: @@ -83,14 +81,10 @@ class ImageSubscriber : public rclcpp::Node // 画像の二値化 cv::inRange( - cv_img->image, - cv::Scalar(LOW_H_1, LOW_S, LOW_V), - cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), + cv_img->image, cv::Scalar(LOW_H_1, LOW_S, LOW_V), cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), img_mask_1); cv::inRange( - cv_img->image, - cv::Scalar(LOW_H_2, LOW_S, LOW_V), - cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), + cv_img->image, cv::Scalar(LOW_H_2, LOW_S, LOW_V), cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), img_mask_2); // マスク画像の合成 @@ -98,16 +92,12 @@ class ImageSubscriber : public rclcpp::Node // ノイズ除去の処理 cv::morphologyEx( - img_thresholded, - img_thresholded, - cv::MORPH_OPEN, + 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, + img_thresholded, img_thresholded, cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); // 画像の検出領域におけるモーメントを計算 @@ -138,9 +128,7 @@ class ImageSubscriber : public rclcpp::Node // カメラの高さを0.44[m]として把持対象物の位置を計算 const double CAMERA_HEIGHT = 0.46; cv::Point3d object_position( - ray.x * CAMERA_HEIGHT, - ray.y * CAMERA_HEIGHT, - ray.z * CAMERA_HEIGHT); + ray.x * CAMERA_HEIGHT, ray.y * CAMERA_HEIGHT, ray.z * CAMERA_HEIGHT); // 把持対象物の位置をTFに配信 geometry_msgs::msg::TransformStamped t; diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp index 66cea244..0470377b 100644 --- a/crane_plus_examples/src/gripper_control.cpp +++ b/crane_plus_examples/src/gripper_control.cpp @@ -25,10 +25,7 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control"); -double to_radians(const double deg_angle) -{ - return deg_angle * M_PI / 180.0; -} +double to_radians(const double deg_angle) { return deg_angle * M_PI / 180.0; } int main(int argc, char ** argv) { @@ -39,10 +36,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_gripper_node); - std::thread([&executor]() {executor.spin();}).detach(); + std::thread([&executor]() { executor.spin(); }).detach(); MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp index 609bc4b5..e7a92bdf 100644 --- a/crane_plus_examples/src/joint_values.cpp +++ b/crane_plus_examples/src/joint_values.cpp @@ -25,10 +25,7 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("joint_values"); -double to_radians(const double deg_angle) -{ - return deg_angle * M_PI / 180.0; -} +double to_radians(const double deg_angle) { return deg_angle * M_PI / 180.0; } int main(int argc, char ** argv) { @@ -39,10 +36,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_node); - std::thread([&executor]() {executor.spin();}).detach(); + std::thread([&executor]() { executor.spin(); }).detach(); MoveGroupInterface move_group_arm(move_group_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setNamedTarget("vertical"); diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp index c5d51168..1c0c9974 100644 --- a/crane_plus_examples/src/pick_and_place.cpp +++ b/crane_plus_examples/src/pick_and_place.cpp @@ -28,10 +28,7 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place"); -double to_radians(const double deg_angle) -{ - return deg_angle * M_PI / 180.0; -} +double to_radians(const double deg_angle) { return deg_angle * M_PI / 180.0; } int main(int argc, char ** argv) { @@ -44,14 +41,14 @@ int main(int argc, char ** argv) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); executor.add_node(move_group_gripper_node); - std::thread([&executor]() {executor.spin();}).detach(); + std::thread([&executor]() { executor.spin(); }).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); double GRIPPER_DEFAULT = 0.0; diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp index f62dd583..d7113606 100644 --- a/crane_plus_examples/src/pick_and_place_tf.cpp +++ b/crane_plus_examples/src/pick_and_place_tf.cpp @@ -29,12 +29,12 @@ #include "geometry_msgs/msg/twist.hpp" #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "std_msgs/msg/string.hpp" #include "tf2/convert.hpp" #include "tf2/exceptions.hpp" -#include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" -#include "std_msgs/msg/string.hpp" +#include "tf2_ros/transform_listener.h" using namespace std::chrono_literals; using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; @@ -42,8 +42,7 @@ class PickAndPlaceTf : public rclcpp::Node { public: PickAndPlaceTf( - rclcpp::Node::SharedPtr move_group_arm_node, - rclcpp::Node::SharedPtr move_group_gripper_node) + rclcpp::Node::SharedPtr move_group_arm_node, rclcpp::Node::SharedPtr move_group_gripper_node) : Node("pick_and_place_tf_node") { using namespace std::placeholders; @@ -87,13 +86,10 @@ class PickAndPlaceTf : public rclcpp::Node // 待機姿勢 control_arm(0.0, 0.0, 0.3, 0, 0, 0); - tf_buffer_ = - std::make_unique(this->get_clock()); - tf_listener_ = - std::make_shared(*tf_buffer_); + tf_buffer_ = std::make_unique(this->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); - timer_ = this->create_wall_timer( - 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); + timer_ = this->create_wall_timer(500ms, std::bind(&PickAndPlaceTf::on_timer, this)); } private: @@ -103,13 +99,9 @@ class PickAndPlaceTf : public rclcpp::Node geometry_msgs::msg::TransformStamped tf_msg; try { - tf_msg = tf_buffer_->lookupTransform( - "base_link", "target_0", - tf2::TimePointZero); + tf_msg = tf_buffer_->lookupTransform("base_link", "target_0", tf2::TimePointZero); } catch (const tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Could not transform base_link to target: %s", - ex.what()); + RCLCPP_INFO(this->get_logger(), "Could not transform base_link to target: %s", ex.what()); return; } @@ -198,8 +190,8 @@ class PickAndPlaceTf : public rclcpp::Node // アーム制御 bool control_arm( - const double x, const double y, const double z, - const double roll, const double pitch, const double yaw) + const double x, const double y, const double z, const double roll, const double pitch, + const double yaw) { geometry_msgs::msg::Pose target_pose; tf2::Quaternion q; @@ -231,9 +223,8 @@ int main(int argc, char ** argv) auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); rclcpp::executors::MultiThreadedExecutor exec; - auto pick_and_place_tf_node = std::make_shared( - move_group_arm_node, - move_group_gripper_node); + auto pick_and_place_tf_node = + std::make_shared(move_group_arm_node, move_group_gripper_node); exec.add_node(pick_and_place_tf_node); exec.add_node(move_group_arm_node); exec.add_node(move_group_gripper_node); diff --git a/crane_plus_examples/src/pose_groupstate.cpp b/crane_plus_examples/src/pose_groupstate.cpp index e5d4c799..f1a4ad0d 100644 --- a/crane_plus_examples/src/pose_groupstate.cpp +++ b/crane_plus_examples/src/pose_groupstate.cpp @@ -32,10 +32,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); - std::thread([&executor]() {executor.spin();}).detach(); + std::thread([&executor]() { executor.spin(); }).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setNamedTarget("home"); diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index ec4ead83..59a23a94 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -66,7 +66,8 @@ def image_callback(self, msg): for i in range(n_markers): # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS + ) # tfの配信 t = TransformStamped() t.header = msg.header diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index d2fa0c14..d8f61dd9 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -31,9 +31,7 @@ def __init__(self): self.camera_info_subscription = self.create_subscription( CameraInfo, 'camera_info', self.camera_info_callback, 10 ) - self.image_thresholded_publisher = self.create_publisher( - Image, 'image_thresholded', 10 - ) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None self.bridge = CvBridge() @@ -58,26 +56,18 @@ def image_callback(self, msg): cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) # 画像の二値化 - img_mask_1 = cv2.inRange( - cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V) - ) - img_mask_2 = cv2.inRange( - cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V) - ) + img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) + img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) # マスク画像の合成 img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) # ノイズ除去の処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx( - img_thresholded, cv2.MORPH_OPEN, kernel - ) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) # 穴埋めの処理 - img_thresholded = cv2.morphologyEx( - img_thresholded, cv2.MORPH_CLOSE, kernel - ) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) # 画像の検出領域におけるモーメントを計算 moment = cv2.moments(img_thresholded) @@ -106,9 +96,11 @@ def image_callback(self, msg): # カメラの高さを0.44[m]として把持対象物の位置を計算 CAMERA_HEIGHT = 0.46 - object_position = [ray.x * CAMERA_HEIGHT, - ray.y * CAMERA_HEIGHT, - ray.z * CAMERA_HEIGHT] + object_position = [ + ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT, + ] # 把持対象物の位置をTFに配信 t = TransformStamped() @@ -120,9 +112,7 @@ def image_callback(self, msg): self.tf_broadcaster.sendTransform(t) # 閾値による二値化画像を配信 - img_thresholded_msg = self.bridge.cv2_to_imgmsg( - img_thresholded, encoding='mono8' - ) + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') self.image_thresholded_publisher.publish(img_thresholded_msg) def camera_info_callback(self, msg): diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 73058387..2d64dd9b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -52,8 +52,12 @@ def main(args=None): plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # armの関節のjoint1〜4を順番に45[deg]ずつ動かす - joint_names = ['crane_plus_joint1', 'crane_plus_joint2', - 'crane_plus_joint3', 'crane_plus_joint4'] + joint_names = [ + 'crane_plus_joint1', + 'crane_plus_joint2', + 'crane_plus_joint3', + 'crane_plus_joint4', + ] target_joint_value = math.radians(45) for joint_name in joint_names: @@ -66,25 +70,13 @@ def main(args=None): joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm'), ) - arm.set_goal_state( - motion_plan_constraints=[joint_constraint] - ) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params - ) + arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) # SRDF内に定義されている'vertical'の姿勢にする arm.set_start_state_to_current_state() arm.set_goal_state(configuration_name='vertical') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params - ) + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 0d5e01fb..ab45f805 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -49,7 +49,7 @@ def set_goal_constraints(x, y, z, roll, pitch, yaw): # 目標位置姿勢の制約設定 goal_constraints = Constraints() - goal_constraints.name = "tolerance_goal" + goal_constraints.name = 'tolerance_goal' # 位置の制約設定 position_constraint = PositionConstraint() @@ -106,18 +106,17 @@ def main(args=None): ) # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # グリッパの開閉角 GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - # 物体を置く位置 # SRDF内に定義されている'vertical'の姿勢にする diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index bc93c5e1..66f13493 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -119,12 +119,9 @@ def on_timer(self): # target_0のtf位置姿勢を取得 tf_msg = TransformStamped() try: - tf_msg = self.tf_buffer.lookup_transform( - 'base_link', 'target_0', Time()) + tf_msg = self.tf_buffer.lookup_transform('base_link', 'target_0', Time()) except tf2_ros.LookupException as ex: - self.get_logger().info( - f'Could not transform base_link to target: {ex}' - ) + self.get_logger().info(f'Could not transform base_link to target: {ex}') now = Time() FILTERING_TIME = datetime.timedelta(seconds=2) @@ -142,13 +139,11 @@ def on_timer(self): # 現在時刻から2秒以内に受け取ったtfを使用 if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: - tf_diff = np.sqrt((self.tf_past.transform.translation.x - - tf_msg.transform.translation.x) ** 2 - + (self.tf_past.transform.translation.y - - tf_msg.transform.translation.y) ** 2 - + (self.tf_past.transform.translation.z - - tf_msg.transform.translation.z) ** 2 - ) + tf_diff = np.sqrt( + (self.tf_past.transform.translation.x - tf_msg.transform.translation.x) ** 2 + + (self.tf_past.transform.translation.y - tf_msg.transform.translation.y) ** 2 + + (self.tf_past.transform.translation.z - tf_msg.transform.translation.z) ** 2 + ) # 把持対象の位置が停止していることを判定 if tf_diff < DISTANCE_THRESHOLD: # 把持対象が3秒以上停止している場合ピッキング動作開始 @@ -178,8 +173,9 @@ def _picking(self, tf_msg): GRIPPER_OFFSET = 0.13 gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) - if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, - 0.05, 0, 90, theta_deg): + if not self._control_arm( + x - gripper_offset_x, y - gripper_offset_y, 0.05, 0, 90, theta_deg + ): # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) return diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index 35ea2af1..bf1faf85 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -13,8 +13,7 @@ # limitations under the License. from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader \ - import RobotDescriptionLoader +from crane_plus_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -50,9 +49,9 @@ def generate_launch_description(): } declare_example_name = DeclareLaunchArgument( - 'example', default_value='color_detection', - description=('Set an example executable name: ' - '[aruco_detection, color_detection]') + 'example', + default_value='color_detection', + description=('Set an example executable name: [aruco_detection, color_detection]'), ) declare_use_sim_time = DeclareLaunchArgument( @@ -71,7 +70,7 @@ def generate_launch_description(): package='crane_plus_examples_py', executable='pick_and_place_tf', output='screen', - parameters=[config_dict] + parameters=[config_dict], ) example_node = Node( @@ -82,10 +81,12 @@ def generate_launch_description(): parameters=[config_dict], ) - return LaunchDescription([ - declare_loaded_description, - declare_example_name, - declare_use_sim_time, - picking_node, - example_node - ]) + return LaunchDescription( + [ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + picking_node, + example_node, + ] + ) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index 3915f40f..cb81ff9f 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -13,8 +13,7 @@ # limitations under the License. from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader \ - import RobotDescriptionLoader +from crane_plus_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -54,7 +53,7 @@ def generate_launch_description(): default_value='gripper_control', description='Set an example executable name: \ [gripper_control, pose_groupstate, \ - joint_values, pick_and_place]' + joint_values, pick_and_place]', ) declare_use_sim_time = DeclareLaunchArgument( @@ -76,9 +75,6 @@ def generate_launch_description(): parameters=[config_dict], ) - return LaunchDescription([ - declare_loaded_description, - declare_example_name, - declare_use_sim_time, - example_node - ]) + return LaunchDescription( + [declare_loaded_description, declare_example_name, declare_use_sim_time, example_node] + ) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 567c4009..646f878b 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -10,13 +10,13 @@ version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), - glob(os.path.join('launch', '*launch.[pxy][yma]'))), - (os.path.join('share', package_name, 'config'), - glob(os.path.join('config', '*'))), + ( + os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]')), + ), + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), ], install_requires=['setuptools'], zip_safe=True, diff --git a/crane_plus_examples_py/test/test_flake8.py b/crane_plus_examples_py/test/test_flake8.py index 27ee1078..d0d8a704 100644 --- a/crane_plus_examples_py/test/test_flake8.py +++ b/crane_plus_examples_py/test/test_flake8.py @@ -20,6 +20,4 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len(errors) + '\n'.join(errors) From 672e3366a05d2430e05beaf766403452fd461351 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 15:05:33 +0900 Subject: [PATCH 092/111] =?UTF-8?q?Revert=20"=E8=87=AA=E5=8B=95=E3=83=95?= =?UTF-8?q?=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88"?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit This reverts commit f68e5cf8510428212a73df9cb9c15ee51a9d0650. --- .../crane_plus_control/crane_plus_driver.hpp | 5 +- .../crane_plus_hardware.hpp | 6 +- crane_plus_control/src/crane_plus_driver.cpp | 64 ++++++++----- .../src/crane_plus_hardware.cpp | 96 +++++++++++++------ .../robot_description_loader.py | 32 +++---- crane_plus_examples/src/aruco_detection.cpp | 30 +++--- crane_plus_examples/src/color_detection.cpp | 40 +++++--- crane_plus_examples/src/gripper_control.cpp | 9 +- crane_plus_examples/src/joint_values.cpp | 9 +- crane_plus_examples/src/pick_and_place.cpp | 11 ++- crane_plus_examples/src/pick_and_place_tf.cpp | 35 ++++--- crane_plus_examples/src/pose_groupstate.cpp | 4 +- .../crane_plus_examples_py/aruco_detection.py | 3 +- .../crane_plus_examples_py/color_detection.py | 32 ++++--- .../crane_plus_examples_py/joint_values.py | 26 +++-- .../crane_plus_examples_py/pick_and_place.py | 11 ++- .../pick_and_place_tf.py | 24 +++-- .../launch/camera_example.launch.py | 27 +++--- .../launch/example.launch.py | 14 ++- crane_plus_examples_py/setup.py | 12 +-- crane_plus_examples_py/test/test_flake8.py | 4 +- 21 files changed, 305 insertions(+), 189 deletions(-) diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp index 71df449e..bae3c1e8 100644 --- a/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp +++ b/crane_plus_control/include/crane_plus_control/crane_plus_driver.hpp @@ -16,7 +16,6 @@ #define CRANE_PLUS_CONTROL__CRANE_PLUS_DRIVER_HPP_ #include - #include #include #include @@ -51,8 +50,8 @@ class CranePlusDriver bool read_byte_data_from_each_joints(const uint16_t address, std::vector & buffer); bool read_word_data_from_each_joints(const uint16_t address, std::vector & buffer); bool parse_dxl_error( - const std::string func_name, const uint8_t dxl_id, const int dxl_comm_result, - const uint8_t dxl_packet_error); + const std::string func_name, const uint8_t dxl_id, + const int dxl_comm_result, const uint8_t dxl_packet_error); double dxl_pos_to_radian(const uint16_t position); uint16_t radian_to_dxl_pos(const double position); double dxl_speed_to_rps(const uint16_t speed); diff --git a/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp b/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp index 64fe66e7..60309cda 100644 --- a/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp +++ b/crane_plus_control/include/crane_plus_control/crane_plus_hardware.hpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. + #ifndef CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ #define CRANE_PLUS_CONTROL__CRANE_PLUS_HARDWARE_HPP_ @@ -30,12 +31,13 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/state.hpp" -using hardware_interface::CallbackReturn; using hardware_interface::return_type; +using hardware_interface::CallbackReturn; namespace crane_plus_control { -class CranePlusHardware : public hardware_interface::SystemInterface +class CranePlusHardware : public + hardware_interface::SystemInterface { public: RCLCPP_SHARED_PTR_DEFINITIONS(CranePlusHardware) diff --git a/crane_plus_control/src/crane_plus_driver.cpp b/crane_plus_control/src/crane_plus_driver.cpp index acc335cf..6d3de2e7 100644 --- a/crane_plus_control/src/crane_plus_driver.cpp +++ b/crane_plus_control/src/crane_plus_driver.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "crane_plus_control/crane_plus_driver.hpp" - #include #include #include #include #include +#include "crane_plus_control/crane_plus_driver.hpp" + constexpr double PROTOCOL_VERSION = 1.0; constexpr int DXL_HOME_POSITION = 511; // value range:0 ~ 1023 constexpr double DXL_MAX_POSITION = 1023.0; @@ -44,7 +44,8 @@ constexpr uint16_t ADDR_PRESENT_VOLTAGE = 42; constexpr uint16_t ADDR_PRESENT_TEMPERATURE = 43; CranePlusDriver::CranePlusDriver( - const std::string port_name, const int baudrate, std::vector id_list) + const std::string port_name, const int baudrate, + std::vector id_list) : baudrate_(baudrate), id_list_(id_list) { dxl_port_handler_ = std::shared_ptr( @@ -53,28 +54,37 @@ CranePlusDriver::CranePlusDriver( dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION)); } -CranePlusDriver::~CranePlusDriver() { close_port(); } +CranePlusDriver::~CranePlusDriver() +{ + close_port(); +} bool CranePlusDriver::open_port(void) { if (!dxl_port_handler_->openPort()) { - last_error_log_ = std::string(__func__) + - ": unable to open dynamixel port: " + dxl_port_handler_->getPortName(); + last_error_log_ = std::string(__func__) + ": unable to open dynamixel port: " + + dxl_port_handler_->getPortName(); return false; } if (!dxl_port_handler_->setBaudRate(baudrate_)) { last_error_log_ = std::string(__func__) + ": unable to set baudrate" + - std::to_string(dxl_port_handler_->getBaudRate()); + std::to_string(dxl_port_handler_->getBaudRate()); return false; } return true; } -void CranePlusDriver::close_port(void) { dxl_port_handler_->closePort(); } +void CranePlusDriver::close_port(void) +{ + dxl_port_handler_->closePort(); +} -std::string CranePlusDriver::get_last_error_log(void) { return last_error_log_; } +std::string CranePlusDriver::get_last_error_log(void) +{ + return last_error_log_; +} bool CranePlusDriver::torque_enable(const bool enable) { @@ -82,7 +92,8 @@ bool CranePlusDriver::torque_enable(const bool enable) for (auto dxl_id : id_list_) { uint8_t dxl_error = 0; int dxl_result = dxl_packet_handler_->write1ByteTxRx( - dxl_port_handler_.get(), dxl_id, ADDR_TORQUE_ENABLE, enable, &dxl_error); + dxl_port_handler_.get(), + dxl_id, ADDR_TORQUE_ENABLE, enable, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -96,8 +107,8 @@ bool CranePlusDriver::write_goal_joint_positions(const std::vector & goa { if (goal_positions.size() != id_list_.size()) { last_error_log_ = std::string(__func__) + ": vectors size does not match: " + - " goal_positions:" + std::to_string(goal_positions.size()) + - ", id_list:" + std::to_string(id_list_.size()); + " goal_positions:" + std::to_string(goal_positions.size()) + + ", id_list:" + std::to_string(id_list_.size()); return false; } @@ -108,7 +119,8 @@ bool CranePlusDriver::write_goal_joint_positions(const std::vector & goa uint16_t goal_position = radian_to_dxl_pos(goal_positions[i]); auto dxl_id = id_list_[i]; int dxl_result = dxl_packet_handler_->write2ByteTxRx( - dxl_port_handler_.get(), dxl_id, ADDR_GOAL_POSITION, goal_position, &dxl_error); + dxl_port_handler_.get(), + dxl_id, ADDR_GOAL_POSITION, goal_position, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -123,7 +135,8 @@ bool CranePlusDriver::write_moving_speed_rpm(const uint8_t dxl_id, const double const int DXL_MAX_MOVING_SPEED = 1023; const double SPEED_UNIT = 0.111; // rpm if (std::find(id_list_.begin(), id_list_.end(), dxl_id) == id_list_.end()) { - last_error_log_ = std::string(__func__) + ": dxl_id: " + std::to_string(dxl_id) + "not found."; + last_error_log_ = std::string(__func__) + ": dxl_id: " + std::to_string(dxl_id) + + "not found."; return false; } @@ -140,7 +153,8 @@ bool CranePlusDriver::write_moving_speed_rpm(const uint8_t dxl_id, const double uint8_t dxl_error = 0; int dxl_result = dxl_packet_handler_->write2ByteTxRx( - dxl_port_handler_.get(), dxl_id, ADDR_MOVING_SPEED, dxl_moving_speed, &dxl_error); + dxl_port_handler_.get(), + dxl_id, ADDR_MOVING_SPEED, dxl_moving_speed, &dxl_error); retval = parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error); @@ -221,14 +235,16 @@ bool CranePlusDriver::read_present_joint_temperatures(std::vector & join } bool CranePlusDriver::read_byte_data_from_each_joints( - const uint16_t address, std::vector & buffer) + const uint16_t address, + std::vector & buffer) { bool retval = true; for (auto dxl_id : id_list_) { uint8_t dxl_error = 0; uint8_t data = 0; int dxl_result = dxl_packet_handler_->read1ByteTxRx( - dxl_port_handler_.get(), dxl_id, address, &data, &dxl_error); + dxl_port_handler_.get(), + dxl_id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -241,14 +257,16 @@ bool CranePlusDriver::read_byte_data_from_each_joints( } bool CranePlusDriver::read_word_data_from_each_joints( - const uint16_t address, std::vector & buffer) + const uint16_t address, + std::vector & buffer) { bool retval = true; for (auto dxl_id : id_list_) { uint8_t dxl_error = 0; uint16_t data = 0; int dxl_result = dxl_packet_handler_->read2ByteTxRx( - dxl_port_handler_.get(), dxl_id, address, &data, &dxl_error); + dxl_port_handler_.get(), + dxl_id, address, &data, &dxl_error); if (!parse_dxl_error(std::string(__func__), dxl_id, dxl_result, dxl_error)) { retval = false; @@ -261,20 +279,20 @@ bool CranePlusDriver::read_word_data_from_each_joints( } bool CranePlusDriver::parse_dxl_error( - const std::string func_name, const uint8_t dxl_id, const int dxl_comm_result, - const uint8_t dxl_packet_error) + const std::string func_name, const uint8_t dxl_id, + const int dxl_comm_result, const uint8_t dxl_packet_error) { bool retval = true; if (dxl_comm_result != COMM_SUCCESS) { last_error_log_ = func_name + ": dxl_id: " + std::to_string(dxl_id) + " :" + - std::string(dxl_packet_handler_->getTxRxResult(dxl_comm_result)); + std::string(dxl_packet_handler_->getTxRxResult(dxl_comm_result)); retval = false; } if (dxl_packet_error != 0) { last_error_log_ = func_name + ": dxl_id: " + std::to_string(dxl_id) + " :" + - std::string(dxl_packet_handler_->getRxPacketError(dxl_packet_error)); + std::string(dxl_packet_handler_->getRxPacketError(dxl_packet_error)); retval = false; } diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index 3857d6a7..6514e000 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -12,15 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "crane_plus_control/crane_plus_hardware.hpp" #include #include #include #include +#include "crane_plus_control/crane_plus_hardware.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" + namespace crane_plus_control { @@ -30,7 +31,8 @@ CranePlusHardware::~CranePlusHardware() driver_->close_port(); } -CallbackReturn CranePlusHardware::on_init(const hardware_interface::HardwareInfo & info) +CallbackReturn CranePlusHardware::on_init( + const hardware_interface::HardwareInfo & info) { if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; @@ -52,7 +54,8 @@ CallbackReturn CranePlusHardware::on_init(const hardware_interface::HardwareInfo dxl_id_list.push_back(std::stoi(joint.parameters["dxl_id"])); } else { RCLCPP_ERROR( - rclcpp::get_logger("CranePlusHardware"), "Joint '%s' does not have 'dxl_id' parameter.", + rclcpp::get_logger("CranePlusHardware"), + "Joint '%s' does not have 'dxl_id' parameter.", joint.name.c_str()); return CallbackReturn::ERROR; } @@ -68,11 +71,13 @@ CallbackReturn CranePlusHardware::on_init(const hardware_interface::HardwareInfo // Open a crane_plus_driver driver_ = std::make_shared(port_name, baudrate, dxl_id_list); if (!driver_->open_port()) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); return CallbackReturn::ERROR; } if (!driver_->torque_enable(false)) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); return CallbackReturn::ERROR; } @@ -81,52 +86,74 @@ CallbackReturn CranePlusHardware::on_init(const hardware_interface::HardwareInfo if (joint.command_interfaces.size() != 1) { RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), - "Joint '%s' has %lu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + "Joint '%s' has %lu command interfaces found. 1 expected.", + joint.name.c_str(), joint.command_interfaces.size()); return CallbackReturn::ERROR; } if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), - "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + "Joint '%s' have %s command interfaces found. '%s' expected.", + joint.name.c_str(), joint.command_interfaces[0].name.c_str(), + hardware_interface::HW_IF_POSITION); return CallbackReturn::ERROR; } } steady_clock_ = rclcpp::Clock(RCL_STEADY_TIME); + return CallbackReturn::SUCCESS; } -std::vector CranePlusHardware::export_state_interfaces() +std::vector +CranePlusHardware::export_state_interfaces() { std::vector state_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_states_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &hw_position_states_[i]) + ); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_velocity_states_[i])); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, + &hw_velocity_states_[i]) + ); state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[i].name, "load", &hw_load_states_[i])); + hardware_interface::StateInterface( + info_.joints[i].name, "load", + &hw_load_states_[i]) + ); + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, "voltage", + &hw_voltage_states_[i]) + ); state_interfaces.emplace_back( - hardware_interface::StateInterface(info_.joints[i].name, "voltage", &hw_voltage_states_[i])); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "temperature", &hw_temperature_states_[i])); + hardware_interface::StateInterface( + info_.joints[i].name, "temperature", + &hw_temperature_states_[i]) + ); } return state_interfaces; } -std::vector CranePlusHardware::export_command_interfaces() +std::vector +CranePlusHardware::export_command_interfaces() { std::vector command_interfaces; for (uint i = 0; i < info_.joints.size(); i++) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_position_commands_[i])); + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, + &hw_position_commands_[i]) + ); } return command_interfaces; @@ -135,7 +162,9 @@ std::vector CranePlusHardware::export_comm CallbackReturn CranePlusHardware::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { if (!driver_->torque_enable(false)) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), + driver_->get_last_error_log().c_str()); return CallbackReturn::ERROR; } // Set current timestamp to disable the communication timeout. @@ -151,7 +180,8 @@ CallbackReturn CranePlusHardware::on_activate(const rclcpp_lifecycle::State & /* return CallbackReturn::SUCCESS; } -CallbackReturn CranePlusHardware::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn CranePlusHardware::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { driver_->torque_enable(false); @@ -163,7 +193,8 @@ return_type CranePlusHardware::read( { if (communication_timeout()) { if (!timeout_has_printed_) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); timeout_has_printed_ = true; } return return_type::ERROR; @@ -171,7 +202,9 @@ return_type CranePlusHardware::read( std::vector joint_positions; if (!driver_->read_present_joint_positions(joint_positions)) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), + driver_->get_last_error_log().c_str()); // readに失敗しても通信は継続させる。 // 不確かなデータをセットしないようにOKを返す。 return return_type::OK; @@ -210,6 +243,7 @@ return_type CranePlusHardware::read( } } + if (read_temperatures_) { std::vector joint_temperatures; if (driver_->read_present_joint_temperatures(joint_temperatures)) { @@ -228,14 +262,17 @@ return_type CranePlusHardware::write( { if (communication_timeout()) { if (!timeout_has_printed_) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), "Communication timeout!"); timeout_has_printed_ = true; } return return_type::ERROR; } if (!driver_->write_goal_joint_positions(hw_position_commands_)) { - RCLCPP_ERROR(rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); + RCLCPP_ERROR( + rclcpp::get_logger("CranePlusHardware"), + driver_->get_last_error_log().c_str()); // writeに失敗しても通信は継続させる。 // 不確かなデータをセットしないようにOKを返す。 return return_type::OK; @@ -258,4 +295,7 @@ bool CranePlusHardware::communication_timeout() #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(crane_plus_control::CranePlusHardware, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS( + crane_plus_control::CranePlusHardware, + hardware_interface::SystemInterface +) diff --git a/crane_plus_description/crane_plus_description/robot_description_loader.py b/crane_plus_description/crane_plus_description/robot_description_loader.py index de543a37..d62ac45f 100755 --- a/crane_plus_description/crane_plus_description/robot_description_loader.py +++ b/crane_plus_description/crane_plus_description/robot_description_loader.py @@ -18,11 +18,13 @@ from launch.substitutions import Command -class RobotDescriptionLoader: +class RobotDescriptionLoader(): + def __init__(self): self.robot_description_path = os.path.join( - get_package_share_directory('crane_plus_description'), 'urdf', 'crane_plus.urdf.xacro' - ) + get_package_share_directory('crane_plus_description'), + 'urdf', + 'crane_plus.urdf.xacro') self.port_name = '/dev/ttyUSB0' self.use_gazebo = 'false' self.use_camera = 'false' @@ -31,21 +33,13 @@ def __init__(self): self.gz_control_config_file_path = '' def load(self): - return Command( - [ + return Command([ 'xacro ', self.robot_description_path, - ' port_name:=', - self.port_name, - ' use_gazebo:=', - self.use_gazebo, - ' use_camera:=', - self.use_camera, - ' use_mock_components:=', - self.use_mock_components, - ' gz_control_config_package:=', - self.gz_control_config_package, - ' gz_control_config_file_path:=', - self.gz_control_config_file_path, - ] - ) + ' port_name:=', self.port_name, + ' use_gazebo:=', self.use_gazebo, + ' use_camera:=', self.use_camera, + ' use_mock_components:=', self.use_mock_components, + ' gz_control_config_package:=', self.gz_control_config_package, + ' gz_control_config_file_path:=', self.gz_control_config_file_path + ]) diff --git a/crane_plus_examples/src/aruco_detection.cpp b/crane_plus_examples/src/aruco_detection.cpp index a6f26ba6..77ca11d3 100644 --- a/crane_plus_examples/src/aruco_detection.cpp +++ b/crane_plus_examples/src/aruco_detection.cpp @@ -18,25 +18,26 @@ #include #include -#include #include +#include -#include "cv_bridge/cv_bridge.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "opencv2/aruco.hpp" -#include "opencv2/opencv.hpp" #include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" -#include "tf2/LinearMath/Matrix3x3.hpp" +#include "opencv2/opencv.hpp" +#include "opencv2/aruco.hpp" +#include "cv_bridge/cv_bridge.hpp" #include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" #include "tf2_ros/transform_broadcaster.h" using std::placeholders::_1; class ImageSubscriber : public rclcpp::Node { public: - ImageSubscriber() : Node("aruco_detection") + ImageSubscriber() + : Node("aruco_detection") { image_subscription_ = this->create_subscription( "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); @@ -44,7 +45,8 @@ class ImageSubscriber : public rclcpp::Node camera_info_subscription_ = this->create_subscription( "camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); - tf_broadcaster_ = std::make_unique(*this); + tf_broadcaster_ = + std::make_unique(*this); } private: @@ -96,10 +98,14 @@ class ImageSubscriber : public rclcpp::Node cv::Mat cv_rotation_matrix; cv::Rodrigues(rvecs[i], cv_rotation_matrix); tf2::Matrix3x3 tf2_rotation_matrix = tf2::Matrix3x3( - cv_rotation_matrix.at(0, 0), cv_rotation_matrix.at(0, 1), - cv_rotation_matrix.at(0, 2), cv_rotation_matrix.at(1, 0), - cv_rotation_matrix.at(1, 1), cv_rotation_matrix.at(1, 2), - cv_rotation_matrix.at(2, 0), cv_rotation_matrix.at(2, 1), + cv_rotation_matrix.at(0, 0), + cv_rotation_matrix.at(0, 1), + cv_rotation_matrix.at(0, 2), + cv_rotation_matrix.at(1, 0), + cv_rotation_matrix.at(1, 1), + cv_rotation_matrix.at(1, 2), + cv_rotation_matrix.at(2, 0), + cv_rotation_matrix.at(2, 1), cv_rotation_matrix.at(2, 2)); tf2_rotation_matrix.getRotation(q); t.transform.rotation.x = q.x(); diff --git a/crane_plus_examples/src/color_detection.cpp b/crane_plus_examples/src/color_detection.cpp index c5e2a878..33e73894 100644 --- a/crane_plus_examples/src/color_detection.cpp +++ b/crane_plus_examples/src/color_detection.cpp @@ -16,27 +16,28 @@ // https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html #include -#include #include +#include #include -#include "cv_bridge/cv_bridge.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "image_geometry/pinhole_camera_model.hpp" -#include "opencv2/imgproc/imgproc.hpp" -#include "opencv2/opencv.hpp" #include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/image.hpp" -#include "tf2/LinearMath/Matrix3x3.hpp" #include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" #include "tf2_ros/transform_broadcaster.h" +#include "opencv2/opencv.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "cv_bridge/cv_bridge.hpp" +#include "image_geometry/pinhole_camera_model.hpp" using std::placeholders::_1; class ImageSubscriber : public rclcpp::Node { public: - ImageSubscriber() : Node("color_detection") + ImageSubscriber() + : Node("color_detection") { image_subscription_ = this->create_subscription( "image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); @@ -47,7 +48,8 @@ class ImageSubscriber : public rclcpp::Node image_thresholded_publisher_ = this->create_publisher("image_thresholded", 10); - tf_broadcaster_ = std::make_unique(*this); + tf_broadcaster_ = + std::make_unique(*this); } private: @@ -81,10 +83,14 @@ class ImageSubscriber : public rclcpp::Node // 画像の二値化 cv::inRange( - cv_img->image, cv::Scalar(LOW_H_1, LOW_S, LOW_V), cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), + cv_img->image, + cv::Scalar(LOW_H_1, LOW_S, LOW_V), + cv::Scalar(HIGH_H_1, HIGH_S, HIGH_V), img_mask_1); cv::inRange( - cv_img->image, cv::Scalar(LOW_H_2, LOW_S, LOW_V), cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), + cv_img->image, + cv::Scalar(LOW_H_2, LOW_S, LOW_V), + cv::Scalar(HIGH_H_2, HIGH_S, HIGH_V), img_mask_2); // マスク画像の合成 @@ -92,12 +98,16 @@ class ImageSubscriber : public rclcpp::Node // ノイズ除去の処理 cv::morphologyEx( - img_thresholded, img_thresholded, cv::MORPH_OPEN, + 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, + img_thresholded, + img_thresholded, + cv::MORPH_CLOSE, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); // 画像の検出領域におけるモーメントを計算 @@ -128,7 +138,9 @@ class ImageSubscriber : public rclcpp::Node // カメラの高さを0.44[m]として把持対象物の位置を計算 const double CAMERA_HEIGHT = 0.46; cv::Point3d object_position( - ray.x * CAMERA_HEIGHT, ray.y * CAMERA_HEIGHT, ray.z * CAMERA_HEIGHT); + ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT); // 把持対象物の位置をTFに配信 geometry_msgs::msg::TransformStamped t; diff --git a/crane_plus_examples/src/gripper_control.cpp b/crane_plus_examples/src/gripper_control.cpp index 0470377b..66cea244 100644 --- a/crane_plus_examples/src/gripper_control.cpp +++ b/crane_plus_examples/src/gripper_control.cpp @@ -25,7 +25,10 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("gripper_control"); -double to_radians(const double deg_angle) { return deg_angle * M_PI / 180.0; } +double to_radians(const double deg_angle) +{ + return deg_angle * M_PI / 180.0; +} int main(int argc, char ** argv) { @@ -36,10 +39,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_gripper_node); - std::thread([&executor]() { executor.spin(); }).detach(); + std::thread([&executor]() {executor.spin();}).detach(); MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); diff --git a/crane_plus_examples/src/joint_values.cpp b/crane_plus_examples/src/joint_values.cpp index e7a92bdf..609bc4b5 100644 --- a/crane_plus_examples/src/joint_values.cpp +++ b/crane_plus_examples/src/joint_values.cpp @@ -25,7 +25,10 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("joint_values"); -double to_radians(const double deg_angle) { return deg_angle * M_PI / 180.0; } +double to_radians(const double deg_angle) +{ + return deg_angle * M_PI / 180.0; +} int main(int argc, char ** argv) { @@ -36,10 +39,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_node); - std::thread([&executor]() { executor.spin(); }).detach(); + std::thread([&executor]() {executor.spin();}).detach(); MoveGroupInterface move_group_arm(move_group_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setNamedTarget("vertical"); diff --git a/crane_plus_examples/src/pick_and_place.cpp b/crane_plus_examples/src/pick_and_place.cpp index 1c0c9974..c5d51168 100644 --- a/crane_plus_examples/src/pick_and_place.cpp +++ b/crane_plus_examples/src/pick_and_place.cpp @@ -28,7 +28,10 @@ using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; static const rclcpp::Logger LOGGER = rclcpp::get_logger("pick_and_place"); -double to_radians(const double deg_angle) { return deg_angle * M_PI / 180.0; } +double to_radians(const double deg_angle) +{ + return deg_angle * M_PI / 180.0; +} int main(int argc, char ** argv) { @@ -41,14 +44,14 @@ int main(int argc, char ** argv) rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); executor.add_node(move_group_gripper_node); - std::thread([&executor]() { executor.spin(); }).detach(); + std::thread([&executor]() {executor.spin();}).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper"); - move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_gripper.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_gripper.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 auto gripper_joint_values = move_group_gripper.getCurrentJointValues(); double GRIPPER_DEFAULT = 0.0; diff --git a/crane_plus_examples/src/pick_and_place_tf.cpp b/crane_plus_examples/src/pick_and_place_tf.cpp index d7113606..f62dd583 100644 --- a/crane_plus_examples/src/pick_and_place_tf.cpp +++ b/crane_plus_examples/src/pick_and_place_tf.cpp @@ -29,12 +29,12 @@ #include "geometry_msgs/msg/twist.hpp" #include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/convert.hpp" #include "tf2/exceptions.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +#include "std_msgs/msg/string.hpp" using namespace std::chrono_literals; using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; @@ -42,7 +42,8 @@ class PickAndPlaceTf : public rclcpp::Node { public: PickAndPlaceTf( - rclcpp::Node::SharedPtr move_group_arm_node, rclcpp::Node::SharedPtr move_group_gripper_node) + rclcpp::Node::SharedPtr move_group_arm_node, + rclcpp::Node::SharedPtr move_group_gripper_node) : Node("pick_and_place_tf_node") { using namespace std::placeholders; @@ -86,10 +87,13 @@ class PickAndPlaceTf : public rclcpp::Node // 待機姿勢 control_arm(0.0, 0.0, 0.3, 0, 0, 0); - tf_buffer_ = std::make_unique(this->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); + tf_buffer_ = + std::make_unique(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); - timer_ = this->create_wall_timer(500ms, std::bind(&PickAndPlaceTf::on_timer, this)); + timer_ = this->create_wall_timer( + 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); } private: @@ -99,9 +103,13 @@ class PickAndPlaceTf : public rclcpp::Node geometry_msgs::msg::TransformStamped tf_msg; try { - tf_msg = tf_buffer_->lookupTransform("base_link", "target_0", tf2::TimePointZero); + tf_msg = tf_buffer_->lookupTransform( + "base_link", "target_0", + tf2::TimePointZero); } catch (const tf2::TransformException & ex) { - RCLCPP_INFO(this->get_logger(), "Could not transform base_link to target: %s", ex.what()); + RCLCPP_INFO( + this->get_logger(), "Could not transform base_link to target: %s", + ex.what()); return; } @@ -190,8 +198,8 @@ class PickAndPlaceTf : public rclcpp::Node // アーム制御 bool control_arm( - const double x, const double y, const double z, const double roll, const double pitch, - const double yaw) + const double x, const double y, const double z, + const double roll, const double pitch, const double yaw) { geometry_msgs::msg::Pose target_pose; tf2::Quaternion q; @@ -223,8 +231,9 @@ int main(int argc, char ** argv) auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options); rclcpp::executors::MultiThreadedExecutor exec; - auto pick_and_place_tf_node = - std::make_shared(move_group_arm_node, move_group_gripper_node); + auto pick_and_place_tf_node = std::make_shared( + move_group_arm_node, + move_group_gripper_node); exec.add_node(pick_and_place_tf_node); exec.add_node(move_group_arm_node); exec.add_node(move_group_gripper_node); diff --git a/crane_plus_examples/src/pose_groupstate.cpp b/crane_plus_examples/src/pose_groupstate.cpp index f1a4ad0d..e5d4c799 100644 --- a/crane_plus_examples/src/pose_groupstate.cpp +++ b/crane_plus_examples/src/pose_groupstate.cpp @@ -32,10 +32,10 @@ int main(int argc, char ** argv) // For current state monitor rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(move_group_arm_node); - std::thread([&executor]() { executor.spin(); }).detach(); + std::thread([&executor]() {executor.spin();}).detach(); MoveGroupInterface move_group_arm(move_group_arm_node, "arm_tcp"); - move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 + move_group_arm.setMaxVelocityScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setMaxAccelerationScalingFactor(1.0); // Set 0.0 ~ 1.0 move_group_arm.setNamedTarget("home"); diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 59a23a94..ec4ead83 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -66,8 +66,7 @@ def image_callback(self, msg): for i in range(n_markers): # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS - ) + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) # tfの配信 t = TransformStamped() t.header = msg.header diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index d8f61dd9..d2fa0c14 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -31,7 +31,9 @@ def __init__(self): self.camera_info_subscription = self.create_subscription( CameraInfo, 'camera_info', self.camera_info_callback, 10 ) - self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) + self.image_thresholded_publisher = self.create_publisher( + Image, 'image_thresholded', 10 + ) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None self.bridge = CvBridge() @@ -56,18 +58,26 @@ def image_callback(self, msg): cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) # 画像の二値化 - img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) - img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) + img_mask_1 = cv2.inRange( + cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V) + ) + img_mask_2 = cv2.inRange( + cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V) + ) # マスク画像の合成 img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) # ノイズ除去の処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + img_thresholded = cv2.morphologyEx( + img_thresholded, cv2.MORPH_OPEN, kernel + ) # 穴埋めの処理 - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + img_thresholded = cv2.morphologyEx( + img_thresholded, cv2.MORPH_CLOSE, kernel + ) # 画像の検出領域におけるモーメントを計算 moment = cv2.moments(img_thresholded) @@ -96,11 +106,9 @@ def image_callback(self, msg): # カメラの高さを0.44[m]として把持対象物の位置を計算 CAMERA_HEIGHT = 0.46 - object_position = [ - ray.x * CAMERA_HEIGHT, - ray.y * CAMERA_HEIGHT, - ray.z * CAMERA_HEIGHT, - ] + object_position = [ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT] # 把持対象物の位置をTFに配信 t = TransformStamped() @@ -112,7 +120,9 @@ def image_callback(self, msg): self.tf_broadcaster.sendTransform(t) # 閾値による二値化画像を配信 - img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') + img_thresholded_msg = self.bridge.cv2_to_imgmsg( + img_thresholded, encoding='mono8' + ) self.image_thresholded_publisher.publish(img_thresholded_msg) def camera_info_callback(self, msg): diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 2d64dd9b..73058387 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -52,12 +52,8 @@ def main(args=None): plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # armの関節のjoint1〜4を順番に45[deg]ずつ動かす - joint_names = [ - 'crane_plus_joint1', - 'crane_plus_joint2', - 'crane_plus_joint3', - 'crane_plus_joint4', - ] + joint_names = ['crane_plus_joint1', 'crane_plus_joint2', + 'crane_plus_joint3', 'crane_plus_joint4'] target_joint_value = math.radians(45) for joint_name in joint_names: @@ -70,13 +66,25 @@ def main(args=None): joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm'), ) - arm.set_goal_state(motion_plan_constraints=[joint_constraint]) - plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + arm.set_goal_state( + motion_plan_constraints=[joint_constraint] + ) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params + ) # SRDF内に定義されている'vertical'の姿勢にする arm.set_start_state_to_current_state() arm.set_goal_state(configuration_name='vertical') - plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params + ) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index ab45f805..0d5e01fb 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -49,7 +49,7 @@ def set_goal_constraints(x, y, z, roll, pitch, yaw): # 目標位置姿勢の制約設定 goal_constraints = Constraints() - goal_constraints.name = 'tolerance_goal' + goal_constraints.name = "tolerance_goal" # 位置の制約設定 position_constraint = PositionConstraint() @@ -106,17 +106,18 @@ def main(args=None): ) # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # グリッパの開閉角 GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) + # 物体を置く位置 # SRDF内に定義されている'vertical'の姿勢にする diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 66f13493..bc93c5e1 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -119,9 +119,12 @@ def on_timer(self): # target_0のtf位置姿勢を取得 tf_msg = TransformStamped() try: - tf_msg = self.tf_buffer.lookup_transform('base_link', 'target_0', Time()) + tf_msg = self.tf_buffer.lookup_transform( + 'base_link', 'target_0', Time()) except tf2_ros.LookupException as ex: - self.get_logger().info(f'Could not transform base_link to target: {ex}') + self.get_logger().info( + f'Could not transform base_link to target: {ex}' + ) now = Time() FILTERING_TIME = datetime.timedelta(seconds=2) @@ -139,11 +142,13 @@ def on_timer(self): # 現在時刻から2秒以内に受け取ったtfを使用 if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: - tf_diff = np.sqrt( - (self.tf_past.transform.translation.x - tf_msg.transform.translation.x) ** 2 - + (self.tf_past.transform.translation.y - tf_msg.transform.translation.y) ** 2 - + (self.tf_past.transform.translation.z - tf_msg.transform.translation.z) ** 2 - ) + tf_diff = np.sqrt((self.tf_past.transform.translation.x + - tf_msg.transform.translation.x) ** 2 + + (self.tf_past.transform.translation.y + - tf_msg.transform.translation.y) ** 2 + + (self.tf_past.transform.translation.z + - tf_msg.transform.translation.z) ** 2 + ) # 把持対象の位置が停止していることを判定 if tf_diff < DISTANCE_THRESHOLD: # 把持対象が3秒以上停止している場合ピッキング動作開始 @@ -173,9 +178,8 @@ def _picking(self, tf_msg): GRIPPER_OFFSET = 0.13 gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) - if not self._control_arm( - x - gripper_offset_x, y - gripper_offset_y, 0.05, 0, 90, theta_deg - ): + if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, + 0.05, 0, 90, theta_deg): # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) return diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py index bf1faf85..35ea2af1 100644 --- a/crane_plus_examples_py/launch/camera_example.launch.py +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -13,7 +13,8 @@ # limitations under the License. from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader import RobotDescriptionLoader +from crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -49,9 +50,9 @@ def generate_launch_description(): } declare_example_name = DeclareLaunchArgument( - 'example', - default_value='color_detection', - description=('Set an example executable name: [aruco_detection, color_detection]'), + 'example', default_value='color_detection', + description=('Set an example executable name: ' + '[aruco_detection, color_detection]') ) declare_use_sim_time = DeclareLaunchArgument( @@ -70,7 +71,7 @@ def generate_launch_description(): package='crane_plus_examples_py', executable='pick_and_place_tf', output='screen', - parameters=[config_dict], + parameters=[config_dict] ) example_node = Node( @@ -81,12 +82,10 @@ def generate_launch_description(): parameters=[config_dict], ) - return LaunchDescription( - [ - declare_loaded_description, - declare_example_name, - declare_use_sim_time, - picking_node, - example_node, - ] - ) + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + picking_node, + example_node + ]) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py index cb81ff9f..3915f40f 100644 --- a/crane_plus_examples_py/launch/example.launch.py +++ b/crane_plus_examples_py/launch/example.launch.py @@ -13,7 +13,8 @@ # limitations under the License. from ament_index_python.packages import get_package_share_directory -from crane_plus_description.robot_description_loader import RobotDescriptionLoader +from crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -53,7 +54,7 @@ def generate_launch_description(): default_value='gripper_control', description='Set an example executable name: \ [gripper_control, pose_groupstate, \ - joint_values, pick_and_place]', + joint_values, pick_and_place]' ) declare_use_sim_time = DeclareLaunchArgument( @@ -75,6 +76,9 @@ def generate_launch_description(): parameters=[config_dict], ) - return LaunchDescription( - [declare_loaded_description, declare_example_name, declare_use_sim_time, example_node] - ) + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + example_node + ]) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 646f878b..567c4009 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -10,13 +10,13 @@ version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ( - os.path.join('share', package_name, 'launch'), - glob(os.path.join('launch', '*launch.[pxy][yma]')), - ), - (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*'))), ], install_requires=['setuptools'], zip_safe=True, diff --git a/crane_plus_examples_py/test/test_flake8.py b/crane_plus_examples_py/test/test_flake8.py index d0d8a704..27ee1078 100644 --- a/crane_plus_examples_py/test/test_flake8.py +++ b/crane_plus_examples_py/test/test_flake8.py @@ -20,4 +20,6 @@ @pytest.mark.linter def test_flake8(): rc, errors = main_with_errors(argv=[]) - assert rc == 0, 'Found %d code style errors / warnings:\n' % len(errors) + '\n'.join(errors) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) From 7e3baea4779878be8c924261bd9a8bf76005967f Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 15:06:55 +0900 Subject: [PATCH 093/111] =?UTF-8?q?ruff=E3=81=A7=E8=87=AA=E5=8B=95?= =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 3 +- .../crane_plus_examples_py/color_detection.py | 32 +++++++------------ .../crane_plus_examples_py/joint_values.py | 26 ++++++--------- .../crane_plus_examples_py/pick_and_place.py | 11 +++---- .../pick_and_place_tf.py | 24 ++++++-------- 5 files changed, 37 insertions(+), 59 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index ec4ead83..59a23a94 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -66,7 +66,8 @@ def image_callback(self, msg): for i in range(n_markers): # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS + ) # tfの配信 t = TransformStamped() t.header = msg.header diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index d2fa0c14..d8f61dd9 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -31,9 +31,7 @@ def __init__(self): self.camera_info_subscription = self.create_subscription( CameraInfo, 'camera_info', self.camera_info_callback, 10 ) - self.image_thresholded_publisher = self.create_publisher( - Image, 'image_thresholded', 10 - ) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) self.tf_broadcaster = tf2_ros.TransformBroadcaster() self.camera_info = None self.bridge = CvBridge() @@ -58,26 +56,18 @@ def image_callback(self, msg): cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) # 画像の二値化 - img_mask_1 = cv2.inRange( - cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V) - ) - img_mask_2 = cv2.inRange( - cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V) - ) + img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) + img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) # マスク画像の合成 img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) # ノイズ除去の処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx( - img_thresholded, cv2.MORPH_OPEN, kernel - ) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) # 穴埋めの処理 - img_thresholded = cv2.morphologyEx( - img_thresholded, cv2.MORPH_CLOSE, kernel - ) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) # 画像の検出領域におけるモーメントを計算 moment = cv2.moments(img_thresholded) @@ -106,9 +96,11 @@ def image_callback(self, msg): # カメラの高さを0.44[m]として把持対象物の位置を計算 CAMERA_HEIGHT = 0.46 - object_position = [ray.x * CAMERA_HEIGHT, - ray.y * CAMERA_HEIGHT, - ray.z * CAMERA_HEIGHT] + object_position = [ + ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT, + ] # 把持対象物の位置をTFに配信 t = TransformStamped() @@ -120,9 +112,7 @@ def image_callback(self, msg): self.tf_broadcaster.sendTransform(t) # 閾値による二値化画像を配信 - img_thresholded_msg = self.bridge.cv2_to_imgmsg( - img_thresholded, encoding='mono8' - ) + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') self.image_thresholded_publisher.publish(img_thresholded_msg) def camera_info_callback(self, msg): diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 73058387..2d64dd9b 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -52,8 +52,12 @@ def main(args=None): plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # armの関節のjoint1〜4を順番に45[deg]ずつ動かす - joint_names = ['crane_plus_joint1', 'crane_plus_joint2', - 'crane_plus_joint3', 'crane_plus_joint4'] + joint_names = [ + 'crane_plus_joint1', + 'crane_plus_joint2', + 'crane_plus_joint3', + 'crane_plus_joint4', + ] target_joint_value = math.radians(45) for joint_name in joint_names: @@ -66,25 +70,13 @@ def main(args=None): joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm'), ) - arm.set_goal_state( - motion_plan_constraints=[joint_constraint] - ) - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params - ) + arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) # SRDF内に定義されている'vertical'の姿勢にする arm.set_start_state_to_current_state() arm.set_goal_state(configuration_name='vertical') - plan_and_execute( - crane_plus, - arm, - logger, - single_plan_parameters=plan_request_params - ) + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) # Finish with error. Related Issue # https://github.com/moveit/moveit2/issues/2693 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 0d5e01fb..ab45f805 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -49,7 +49,7 @@ def set_goal_constraints(x, y, z, roll, pitch, yaw): # 目標位置姿勢の制約設定 goal_constraints = Constraints() - goal_constraints.name = "tolerance_goal" + goal_constraints.name = 'tolerance_goal' # 位置の制約設定 position_constraint = PositionConstraint() @@ -106,18 +106,17 @@ def main(args=None): ) # 動作速度の調整 - arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # グリッパの開閉角 GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - # 物体を置く位置 # SRDF内に定義されている'vertical'の姿勢にする diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index bc93c5e1..66f13493 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -119,12 +119,9 @@ def on_timer(self): # target_0のtf位置姿勢を取得 tf_msg = TransformStamped() try: - tf_msg = self.tf_buffer.lookup_transform( - 'base_link', 'target_0', Time()) + tf_msg = self.tf_buffer.lookup_transform('base_link', 'target_0', Time()) except tf2_ros.LookupException as ex: - self.get_logger().info( - f'Could not transform base_link to target: {ex}' - ) + self.get_logger().info(f'Could not transform base_link to target: {ex}') now = Time() FILTERING_TIME = datetime.timedelta(seconds=2) @@ -142,13 +139,11 @@ def on_timer(self): # 現在時刻から2秒以内に受け取ったtfを使用 if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: - tf_diff = np.sqrt((self.tf_past.transform.translation.x - - tf_msg.transform.translation.x) ** 2 - + (self.tf_past.transform.translation.y - - tf_msg.transform.translation.y) ** 2 - + (self.tf_past.transform.translation.z - - tf_msg.transform.translation.z) ** 2 - ) + tf_diff = np.sqrt( + (self.tf_past.transform.translation.x - tf_msg.transform.translation.x) ** 2 + + (self.tf_past.transform.translation.y - tf_msg.transform.translation.y) ** 2 + + (self.tf_past.transform.translation.z - tf_msg.transform.translation.z) ** 2 + ) # 把持対象の位置が停止していることを判定 if tf_diff < DISTANCE_THRESHOLD: # 把持対象が3秒以上停止している場合ピッキング動作開始 @@ -178,8 +173,9 @@ def _picking(self, tf_msg): GRIPPER_OFFSET = 0.13 gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) - if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, - 0.05, 0, 90, theta_deg): + if not self._control_arm( + x - gripper_offset_x, y - gripper_offset_y, 0.05, 0, 90, theta_deg + ): # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) return From 91c962c5d99f22a4e8d34e48e9852bcf2ffb3f25 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 15:41:16 +0900 Subject: [PATCH 094/111] =?UTF-8?q?import=E6=96=87=E3=81=AE=E9=A0=86?= =?UTF-8?q?=E5=BA=8F=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place.py | 7 +++---- .../crane_plus_examples_py/pick_and_place_tf.py | 2 +- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index ab45f805..1f7713eb 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -15,18 +15,17 @@ import math from crane_plus_examples_py.utils import plan_and_execute -from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from geometry_msgs.msg import PoseStamped from moveit.core.robot_state import RobotState -from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume -from shape_msgs.msg import SolidPrimitive from moveit.planning import ( MoveItPy, PlanRequestParameters, ) - +from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume import rclpy from rclpy.logging import get_logger from scipy.spatial.transform import Rotation +from shape_msgs.msg import SolidPrimitive def set_goal_constraints(x, y, z, roll, pitch, yaw): diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 66f13493..c6a7e977 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -33,10 +33,10 @@ from rclpy.logging import get_logger from rclpy.node import Node from rclpy.time import Time +from scipy.spatial.transform import Rotation import tf2_ros from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer -from scipy.spatial.transform import Rotation class PickAndPlaceTf(Node): From 1f592858d29bdfd4140f5c63b848f8e3aac931e2 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 15:50:53 +0900 Subject: [PATCH 095/111] =?UTF-8?q?import=E9=A0=86=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 --- crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index 1f7713eb..d71a5ed0 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -21,7 +21,7 @@ MoveItPy, PlanRequestParameters, ) -from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint, BoundingVolume +from moveit_msgs.msg import BoundingVolume, Constraints, OrientationConstraint, PositionConstraint import rclpy from rclpy.logging import get_logger from scipy.spatial.transform import Rotation From b930d61d1599601c3286d205f48bbdb96b2978fe Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 17:15:41 +0900 Subject: [PATCH 096/111] =?UTF-8?q?=E8=89=B2=E6=A4=9C=E5=87=BA=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E3=81=8C=E5=8B=95=E3=81=8F=E3=82=88?= =?UTF-8?q?=E3=81=86=E3=81=AB=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/color_detection.py | 10 +-- .../pick_and_place_tf.py | 84 ++++++++++++++----- 2 files changed, 68 insertions(+), 26 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index d8f61dd9..f437fbc3 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -32,7 +32,7 @@ def __init__(self): CameraInfo, 'camera_info', self.camera_info_callback, 10 ) self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) - self.tf_broadcaster = tf2_ros.TransformBroadcaster() + self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) self.camera_info = None self.bridge = CvBridge() @@ -89,7 +89,7 @@ def image_callback(self, msg): point = (pixel_x, pixel_y) # 補正後の画像座標系における把持対象物の位置を取得（2D） - rect_point = camera_model.rectifyImage(point) + rect_point = camera_model.rectifyPoint(point) # カメラ座標系から見た把持対象物の方向（Ray）を取得する ray = camera_model.projectPixelTo3dRay(rect_point) @@ -97,9 +97,9 @@ def image_callback(self, msg): # カメラの高さを0.44[m]として把持対象物の位置を計算 CAMERA_HEIGHT = 0.46 object_position = [ - ray.x * CAMERA_HEIGHT, - ray.y * CAMERA_HEIGHT, - ray.z * CAMERA_HEIGHT, + ray[0] * CAMERA_HEIGHT, + ray[1] * CAMERA_HEIGHT, + ray[2] * CAMERA_HEIGHT, ] # 把持対象物の位置をTFに配信 diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index c6a7e977..0819d988 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -25,15 +25,21 @@ MoveItPy, PlanRequestParameters, ) -from moveit_msgs.msg import Constraints, JointConstraint +from moveit_msgs.msg import ( + BoundingVolume, + Constraints, + JointConstraint, + OrientationConstraint, + PositionConstraint, +) import numpy as np import rclpy from rclpy.logging import get_logger from rclpy.node import Node -from rclpy.time import Time from scipy.spatial.transform import Rotation +from shape_msgs.msg import SolidPrimitive import tf2_ros from tf2_ros import TransformListener, TransformStamped from tf2_ros.buffer import Buffer @@ -119,35 +125,38 @@ def on_timer(self): # target_0のtf位置姿勢を取得 tf_msg = TransformStamped() try: - tf_msg = self.tf_buffer.lookup_transform('base_link', 'target_0', Time()) + tf_msg = self.tf_buffer.lookup_transform( + 'crane_plus_base', 'target_0', rclpy.time.Time() + ) except tf2_ros.LookupException as ex: self.get_logger().info(f'Could not transform base_link to target: {ex}') - now = Time() - FILTERING_TIME = datetime.timedelta(seconds=2) - STOP_TIME_THRESHOLD = datetime.timedelta(seconds=3) + now_time = self.get_clock().now() + FILTERING_TIME = rclpy.duration.Duration(seconds=2) + STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3) DISTANCE_THRESHOLD = 0.01 # 経過時間と停止時間を計算(nsec) # 経過時間 - TF_ELAPSED_TIME = now.nanoseconds - tf_msg.header.stamp.nanosec + tf_time = rclpy.time.Time.from_msg(tf_msg.header.stamp) + TF_ELAPSED_TIME = now_time - tf_time # 停止時間 - if self.tf_past is not None: - TF_STOP_TIME = now.nanoseconds - self.tf_past.header.stamp.nanosec - else: - TF_STOP_TIME = now.nanoseconds + tf_past_time = rclpy.time.Time.from_msg(self.tf_past.header.stamp) + TF_STOP_TIME = now_time - tf_past_time # 現在時刻から2秒以内に受け取ったtfを使用 - if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: - tf_diff = np.sqrt( - (self.tf_past.transform.translation.x - tf_msg.transform.translation.x) ** 2 - + (self.tf_past.transform.translation.y - tf_msg.transform.translation.y) ** 2 - + (self.tf_past.transform.translation.z - tf_msg.transform.translation.z) ** 2 + if TF_ELAPSED_TIME < FILTERING_TIME: + tf_diff = np.linalg.norm( + [ + self.tf_past.transform.translation.x - tf_msg.transform.translation.x, + self.tf_past.transform.translation.y - tf_msg.transform.translation.y, + self.tf_past.transform.translation.z - tf_msg.transform.translation.z, + ] ) # 把持対象の位置が停止していることを判定 if tf_diff < DISTANCE_THRESHOLD: # 把持対象が3秒以上停止している場合ピッキング動作開始 - if TF_STOP_TIME > STOP_TIME_THRESHOLD.total_seconds() * 1e9: + if TF_STOP_TIME > STOP_TIME_THRESHOLD: self._picking(tf_msg) else: self.tf_past = tf_msg @@ -215,6 +224,11 @@ def _control_gripper(self, angle): # アーム制御 def _control_arm(self, x, y, z, roll, pitch, yaw): + # 位置姿勢の許容誤差 + POSITION_TOLERANCE = 0.00001 + ORIENTATION_TOLERANCE = 0.0001 + + # 目標位置姿勢 target_pose = PoseStamped() target_pose.header.frame_id = 'crane_plus_base' target_pose.pose.position.x = x @@ -226,14 +240,42 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose.pose.orientation.y = quat[1] target_pose.pose.orientation.z = quat[2] target_pose.pose.orientation.w = quat[3] + + # 目標位置姿勢の制約設定 + goal_constraints = Constraints() + goal_constraints.name = 'tolerance_goal' + + # 位置の制約設定 + position_constraint = PositionConstraint() + position_constraint.header.frame_id = 'crane_plus_base' + position_constraint.link_name = 'crane_plus_link4' + tolerance_region = BoundingVolume() + primitive = SolidPrimitive() + primitive.type = SolidPrimitive.SPHERE + primitive.dimensions = [POSITION_TOLERANCE] + tolerance_region.primitives.append(primitive) + tolerance_region.primitive_poses.append(target_pose.pose) + position_constraint.constraint_region = tolerance_region + position_constraint.weight = 1.0 + + # 姿勢の制約設定 + orientation_constraint = OrientationConstraint() + orientation_constraint.header.frame_id = 'crane_plus_base' + orientation_constraint.link_name = 'crane_plus_link4' + orientation_constraint.orientation = target_pose.pose.orientation + orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.absolute_z_axis_tolerance = ORIENTATION_TOLERANCE + orientation_constraint.weight = 1.0 + + goal_constraints.position_constraints.append(position_constraint) + goal_constraints.orientation_constraints.append(orientation_constraint) + self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state( - pose_stamped_msg=target_pose, pose_link='crane_plus_link4' - ) + self.crane_plus_arm.set_goal_state(motion_plan_constraints=[goal_constraints]) result = plan_and_execute( self.crane_plus, self.crane_plus_arm, - # logger=None, self.logger, single_plan_parameters=self.arm_plan_request_params, ) From 70e87cfc5ac980ba9aea6868c7a03d8d4052dbd3 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 17:55:44 +0900 Subject: [PATCH 097/111] =?UTF-8?q?ArUco=E3=83=9E=E3=83=BC=E3=82=AB?= =?UTF-8?q?=E6=A4=9C=E5=87=BA=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E3=81=8C?= =?UTF-8?q?=E5=8B=95=E4=BD=9C=E3=81=99=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 52 ++++++++++--------- crane_plus_examples_py/setup.py | 2 +- 2 files changed, 28 insertions(+), 26 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 59a23a94..3368ce86 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -33,9 +33,10 @@ def __init__(self): self.camera_info_subscription = self.create_subscription( CameraInfo, 'camera_info', self.camera_info_callback, 10 ) - self.tf_broadcaster = tf2_ros.TransformBroadcaster() + self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) self.camera_info = None + self.bridge = CvBridge() def image_callback(self, msg): @@ -53,36 +54,37 @@ def image_callback(self, msg): corners = [] # マーカの検出 corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + if ids is None: + return # マーカの検出数 n_markers = len(ids) # カメラパラメータ - CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) - DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) + CAMERA_MATRIX = np.array(self.camera_info.k).reshape(3, 3) + DIST_COEFFS = np.array(self.camera_info.d).reshape(1, 5) # マーカ一辺の長さ 0.04 [m] MARKER_LENGTH = 0.04 + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS + ) - # マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 - if n_markers > 0: - for i in range(n_markers): - # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 - rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS - ) - # tfの配信 - t = TransformStamped() - t.header = msg.header - t.child_frame_id = 'target_' + str(ids[i][0]) - t.transform.translation.x = tvecs[i][0][0] - t.transform.translation.y = tvecs[i][0][1] - t.transform.translation.z = tvecs[i][0][2] - # 回転ベクトルをクォータニオンに変換 - marker_orientation_rot = Rotation.from_rotvec(rvecs) - marker_orientation_quat = marker_orientation_rot.as_quat() - t.transform.rotation.x = marker_orientation_quat[0] - t.transform.rotation.y = marker_orientation_quat[1] - t.transform.rotation.z = marker_orientation_quat[2] - t.transform.rotation.w = marker_orientation_quat[3] - self.tf_broadcaster.sendTransform(t) + # マーカの位置姿勢をtfで配信 + for i in range(n_markers): + # tfの配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = 'target_' + str(ids[i][0]) + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトルをクォータニオンに変換 + marker_orientation_rot = Rotation.from_rotvec(rvecs[i][0]) + marker_orientation_quat = marker_orientation_rot.as_quat() + t.transform.rotation.x = marker_orientation_quat[0] + t.transform.rotation.y = marker_orientation_quat[1] + t.transform.rotation.z = marker_orientation_quat[2] + t.transform.rotation.w = marker_orientation_quat[3] + self.tf_broadcaster.sendTransform(t) def camera_info_callback(self, msg): self.camera_info = msg diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 567c4009..67316862 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -27,7 +27,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'aruco_detection = crane_plus_examples_py.aruco_detectio:main', + 'aruco_detection = crane_plus_examples_py.aruco_detection:main', 'color_detection = crane_plus_examples_py.color_detection:main', 'gripper_control = crane_plus_examples_py.gripper_control:main', 'joint_values = crane_plus_examples_py.joint_values:main', From e6d8a09672fa8f08607a0c79fa767fa44a609c97 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 18:13:28 +0900 Subject: [PATCH 098/111] =?UTF-8?q?README=E3=82=92crane=5Fx7=5Fros?= =?UTF-8?q?=E3=81=AB=E5=90=88=E3=82=8F=E3=81=9B=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/README.md | 70 ++++++++------------------------ 1 file changed, 17 insertions(+), 53 deletions(-) diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md index 000e3676..7e8168c2 100644 --- a/crane_plus_examples_py/README.md +++ b/crane_plus_examples_py/README.md @@ -2,59 +2,23 @@ このパッケージはCRANE+ V2 ROS 2パッケージのPythonによるサンプルコード集です。 -## 準備（実機を使う場合） - -![crane_plus](https://rt-net.github.io/images/crane-plus/CRANEV2-500x500.png) - -### 1. CRANE+ V2本体をPCに接続する - -CRANE+ V2本体をPCに接続します。 -接続方法は製品マニュアルを参照してください。 - -**※CRANE+ V2本体が接触しないように、十分なスペースを確保してください。** - -### 2. USB通信ポートの接続を確認する - -USB通信ポートの設定については`crane_plus_control`の -[README](../crane_plus_control/README.md) -を参照してください。 - -**正しく設定できていない場合、CRANE+ V2が動作しない、振動する、などの不安定な動きになるので注意してください** - -### 3. move_groupとcontrollerを起動する - -#### 標準のCRANE+ V2を使用する場合 - -次のコマンドでmove_group (`crane_plus_moveit_config`)と -controller (`crane_plus_control`)を起動します。 - -```sh -$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 -``` - -#### Webカメラ搭載モデルを使用する場合 - -Webカメラ搭載モデルの場合は、次のコマンドを実行してください。 -```video_device```は使用するWebカメラを指定してください。 - -```sh -$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 use_camera:=true video_device:=/dev/video0 -``` - -## 準備（Gazeboを使う場合） -======= -![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png) - -### 1. move_groupとGazeboを起動する - -次のコマンドでmove_group (`crane_plus_moveit_config`)とGazeboを起動します。 - -```sh -$ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py -``` - -## Pythonのサンプルプログラムを実行する - +- [crane\_plus\_examples\_py](#crane_plus_examples_py) + - [起動方法](#起動方法) + - [サンプルプログラムを実行する](#サンプルプログラムを実行する) + - [Gazeboでサンプルプログラムを実行する場合](#Gazeboでサンプルプログラムを実行する場合) + - [Examples](#examples) + - [gripper\_control](#gripper_control) + - [pose\_groupstate](#pose_groupstate) + - [joint\_values](#joint_values) + - [pick\_and\_place](#pick_and_place) + - [Camera Examples](#camera-examples) + - [aruco\_detection](#aruco_detection) + - [color\_detection](#color_detection) + +## 起動方法 +CRANE+ V2の起動方法は[crane_plus_examplesのREADME](../crane_plus_examples/README.md)を参照してください。 + +## サンプルプログラムを実行する 準備ができたらPythonによるサンプルプログラムを実行します。 例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 From 99574faa30139ed7c20f8dc00a149298fcbe15cd Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 18:15:06 +0900 Subject: [PATCH 099/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AAimport?= =?UTF-8?q?=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place_tf.py | 1 - 1 file changed, 1 deletion(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 0819d988..7ff940bd 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -12,7 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import datetime import math from crane_plus_examples_py.utils import plan_and_execute From 81054bf9e2f46622f9a1382a0a72f0803abb2151 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 6 Feb 2026 18:24:32 +0900 Subject: [PATCH 100/111] =?UTF-8?q?arm=E3=82=B0=E3=83=AB=E3=83=BC=E3=83=97?= =?UTF-8?q?=E3=81=8B=E3=82=89arm=5Ftcp=E3=82=B0=E3=83=AB=E3=83=BC=E3=83=97?= =?UTF-8?q?=E3=81=B8=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/crane_plus_examples_py/joint_values.py | 4 ++-- .../crane_plus_examples_py/pose_groupstate.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 2d64dd9b..90d54819 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -36,7 +36,7 @@ def main(args=None): logger.info('MoveItPy instance created') # アーム制御用 planning component - arm = crane_plus.get_planning_component('arm') + arm = crane_plus.get_planning_component('arm_tcp') # instantiate a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() @@ -67,7 +67,7 @@ def main(args=None): robot_state.joint_positions = joint_values joint_constraint = construct_joint_constraint( robot_state=robot_state, - joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm'), + joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm_tcp'), ) arm.set_goal_state(motion_plan_constraints=[joint_constraint]) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py index 428bf700..cef4555c 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -32,7 +32,7 @@ def main(args=None): logger.info('MoveItPy instance created') # アーム制御用 planning component - arm = crane_plus.get_planning_component('arm') + arm = crane_plus.get_planning_component('arm_tcp') plan_request_params = PlanRequestParameters( crane_plus, From ab5bdec970d576c057a023c0cc8c1640b289296d Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 9 Feb 2026 12:34:06 +0900 Subject: [PATCH 101/111] =?UTF-8?q?tcp=E3=83=AA=E3=83=B3=E3=82=AF=E3=82=92?= =?UTF-8?q?=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place.py | 23 +++++++++----- .../pick_and_place_tf.py | 30 +++++++++---------- 2 files changed, 30 insertions(+), 23 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index d71a5ed0..d8737a85 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -53,7 +53,7 @@ def set_goal_constraints(x, y, z, roll, pitch, yaw): # 位置の制約設定 position_constraint = PositionConstraint() position_constraint.header.frame_id = 'crane_plus_base' - position_constraint.link_name = 'crane_plus_link4' + position_constraint.link_name = 'crane_plus_link_tcp' tolerance_region = BoundingVolume() primitive = SolidPrimitive() primitive.type = SolidPrimitive.SPHERE @@ -66,7 +66,7 @@ def set_goal_constraints(x, y, z, roll, pitch, yaw): # 姿勢の制約設定 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = 'crane_plus_base' - orientation_constraint.link_name = 'crane_plus_link4' + orientation_constraint.link_name = 'crane_plus_link_tcp' orientation_constraint.orientation = target_pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE @@ -89,7 +89,7 @@ def main(args=None): logger.info('MoveItPy instance created') # アーム制御用 planning component - arm = crane_plus.get_planning_component('arm') + arm = crane_plus.get_planning_component('arm_tcp') # グリッパ制御用 planning component gripper = crane_plus.get_planning_component('gripper') @@ -164,7 +164,16 @@ def main(args=None): # 物体の上に腕を伸ばす arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.09, 0.17, 0.0, 90.0, -90.0) + goal_constraints = set_goal_constraints(0.0, -0.21, 0.17, 0.0, 90.0, -90.0) + arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + arm.set_start_state_to_current_state() + goal_constraints = set_goal_constraints(0.0, -0.1, 0.05, 0.0, 180.0, -90.0) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -175,7 +184,7 @@ def main(args=None): # 掴みに行く arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.09, 0.14, 0.0, 180.0, -90.0) + goal_constraints = set_goal_constraints(0.0, -0.1, 0.02, 0.0, 180.0, -90.0) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -198,7 +207,7 @@ def main(args=None): # 持ち上げる arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.09, 0.17, 0.0, 90.0, -90.0) + goal_constraints = set_goal_constraints(0.0, -0.1, 0.05, 0.0, 180.0, -90.0) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -219,7 +228,7 @@ def main(args=None): # 下ろす arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.15, 0.0, 0.06, 0.0, 90.0, 0.0) + goal_constraints = set_goal_constraints(0.25, 0.0, 0.06, 0.0, 90.0, 0.0) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index 7ff940bd..d6b353c8 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -50,7 +50,7 @@ def __init__(self, crane_plus): self.logger = get_logger('pick_and_place_tf') self.tf_past = TransformStamped() self.crane_plus = crane_plus - self.crane_plus_arm = crane_plus.get_planning_component('arm') + self.crane_plus_arm = crane_plus.get_planning_component('arm_tcp') self.crane_plus_gripper = crane_plus.get_planning_component('gripper') # instantiate a RobotState instance using the current robot model self.robot_model = crane_plus.get_robot_model() @@ -111,7 +111,7 @@ def __init__(self, crane_plus): self.crane_plus_arm.set_path_constraints(constraints) # 待機姿勢 - self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) # tf self.tf_buffer = Buffer() @@ -175,36 +175,34 @@ def _picking(self, tf_msg): theta_deg = math.degrees(theta_rad) # 把持対象物に正対する - self._control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg) + self._control_arm(0.0, 0.0, 0.3, 0, 0, theta_deg) # 掴みに行く - GRIPPER_OFFSET = 0.13 - gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) - gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) - if not self._control_arm( - x - gripper_offset_x, y - gripper_offset_y, 0.05, 0, 90, theta_deg - ): + if not self._control_arm(x, y, 0.04, 0, 90, theta_deg): # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る - self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) return # ハンドを閉じる self._control_gripper(GRIPPER_CLOSE) # 移動する - self._control_arm(0.0, 0.0, 0.17, 0, 90, 0) + self._control_arm(0.12, 0.0, 0.17, 0, 90, 0) + + # 横へ移動する + self._control_arm(0.0, -0.12, 0.17, 0, 90, -90) # 下ろす - self._control_arm(0.0, -0.15, 0.06, 0, 90, -90) + self._control_arm(0.0, -0.25, 0.05, 0, 90, -90) # ハンドを開く self._control_gripper(GRIPPER_OPEN) # 少しだけハンドを持ち上げる - self._control_arm(0.0, -0.15, 0.10, 0, 90, -90) + self._control_arm(0.0, -0.25, 0.10, 0, 90, -90) # 待機姿勢に戻る - self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) # ハンドを閉じる self._control_gripper(GRIPPER_DEFAULT) @@ -247,7 +245,7 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): # 位置の制約設定 position_constraint = PositionConstraint() position_constraint.header.frame_id = 'crane_plus_base' - position_constraint.link_name = 'crane_plus_link4' + position_constraint.link_name = 'crane_plus_link_tcp' tolerance_region = BoundingVolume() primitive = SolidPrimitive() primitive.type = SolidPrimitive.SPHERE @@ -260,7 +258,7 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): # 姿勢の制約設定 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = 'crane_plus_base' - orientation_constraint.link_name = 'crane_plus_link4' + orientation_constraint.link_name = 'crane_plus_link_tcp' orientation_constraint.orientation = target_pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = ORIENTATION_TOLERANCE orientation_constraint.absolute_y_axis_tolerance = ORIENTATION_TOLERANCE From e1053de9e860aa9e8265faedc66e1dd849c6626a Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 9 Feb 2026 13:06:44 +0900 Subject: [PATCH 102/111] =?UTF-8?q?=E5=AE=9F=E8=A1=8C=E3=82=B3=E3=83=9E?= =?UTF-8?q?=E3=83=B3=E3=83=89=E3=81=AE=E5=85=88=E9=A0=AD=E3=81=AE$?= =?UTF-8?q?=E3=83=9E=E3=83=BC=E3=82=AF=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/README.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md index 7e8168c2..e0a86981 100644 --- a/crane_plus_examples_py/README.md +++ b/crane_plus_examples_py/README.md @@ -23,7 +23,7 @@ CRANE+ V2の起動方法は[crane_plus_examplesのREADME](../crane_plus_examples 例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 ```sh -$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' ``` 終了するときは`Ctrl+c`を入力します。 @@ -33,7 +33,7 @@ $ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 ```sh -$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true +ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true ``` ## Examples @@ -65,7 +65,7 @@ Arguments (pass arguments as ':='): 次のコマンドを実行します。 ```sh -$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' ``` @@ -84,7 +84,7 @@ SRDFファイル[crane_plus_moveit_config/config/crane_plus.srdf](../crane_plus_ 次のコマンドを実行します。 ```sh -$ ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate' +ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate' ``` @@ -100,7 +100,7 @@ $ ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate 次のコマンドを実行します。 ```sh -$ ros2 launch crane_plus_examples_py example.launch.py example:='joint_values' +ros2 launch crane_plus_examples_py example.launch.py example:='joint_values' ``` @@ -116,7 +116,7 @@ $ ros2 launch crane_plus_examples_py example.launch.py example:='joint_values' 次のコマンドを実行します。 ```sh -$ ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place' +ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place' ``` From 8e425a02da3f01b0446d39ce314049393dfde2f0 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 9 Feb 2026 13:23:27 +0900 Subject: [PATCH 103/111] =?UTF-8?q?=E3=83=91=E3=83=83=E3=82=B1=E3=83=BC?= =?UTF-8?q?=E3=82=B8=E6=83=85=E5=A0=B1=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/setup.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index 67316862..352becd4 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -7,7 +7,7 @@ setup( name=package_name, - version='0.1.0', + version='3.0.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', @@ -20,10 +20,10 @@ ], install_requires=['setuptools'], zip_safe=True, - maintainer='user', - maintainer_email='user@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='RT Corporation', + maintainer_email='shop@rt-net.jp', + description='python examples of CRANE+ V2 ROS package', + license='Apache License 2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ From 952beef021f339e118adb590e37027df9fe071b6 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Thu, 21 May 2026 16:24:11 +0900 Subject: [PATCH 104/111] =?UTF-8?q?=E3=81=93=E3=81=93=E3=81=AB=E3=83=A9?= =?UTF-8?q?=E3=82=A4=E3=82=BB=E3=83=B3=E3=82=B9=E3=83=95=E3=82=A1=E3=82=A4?= =?UTF-8?q?=E3=83=AB=E3=81=AF=E4=B8=8D=E8=A6=81=E3=81=AA=E3=81=AE=E3=81=A7?= =?UTF-8?q?=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/LICENSE | 202 --------------------------------- 1 file changed, 202 deletions(-) delete mode 100644 crane_plus_examples_py/LICENSE diff --git a/crane_plus_examples_py/LICENSE b/crane_plus_examples_py/LICENSE deleted file mode 100644 index d6456956..00000000 --- a/crane_plus_examples_py/LICENSE +++ /dev/null @@ -1,202 +0,0 @@ - - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. From 88c300eb8225f95b70d8cc4d5943cfdf54c83461 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Thu, 21 May 2026 17:20:15 +0900 Subject: [PATCH 105/111] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88=E3=81=AB=E5=90=88=E3=82=8F=E3=81=9B=E3=81=A6?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 12 +++++- crane_plus_examples_py/README.md | 69 +++++++++++++++++--------------- 2 files changed, 47 insertions(+), 34 deletions(-) diff --git a/README.md b/README.md index 2a3849f9..34e39a14 100644 --- a/README.md +++ b/README.md @@ -95,7 +95,10 @@ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' - CRANE+ V2のモデルデータ（xacro）を定義するパッケージです - crane_plus_examples - [README](./crane_plus_examples/README.md) - - CRANE+ V2のサンプルコード集です + - CRANE+ V2のC++サンプルコード集です +- crane_plus_examples_py + - [README](./crane_plus_examples_py/README.md) + - CRANE+ V2のPythonサンプルコード集です - crane_plus_gazebo - [README](./crane_plus_gazebo/README.md) - CRANE+ V2のGazeboシミュレーションパッケージです @@ -105,7 +108,12 @@ ros2 launch crane_plus_examples example.launch.py example:='gripper_control' ## How to Use Examples -サンプルプログラムは、`crane_plus_examples`パッケージの[README](./crane_plus_description/README.md)参照してください。 +サンプルプログラムは、C++とPythonの両方を用意しています。詳しくは、以下のリンクをご覧ください。 + +- C++ + - [crane_plus_examples](./crane_plus_examples/README.md) +- Python + - [crane_plus_examples_py](./crane_plus_examples_py/README.md) ## License diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md index e0a86981..84a15342 100644 --- a/crane_plus_examples_py/README.md +++ b/crane_plus_examples_py/README.md @@ -2,10 +2,12 @@ このパッケージはCRANE+ V2 ROS 2パッケージのPythonによるサンプルコード集です。 +## Table of Contents + - [crane\_plus\_examples\_py](#crane_plus_examples_py) - - [起動方法](#起動方法) - - [サンプルプログラムを実行する](#サンプルプログラムを実行する) - - [Gazeboでサンプルプログラムを実行する場合](#Gazeboでサンプルプログラムを実行する場合) + - [Table of Contents](#table-of-contents) + - [Setup](#setup) + - [How to Run Examples](#how-to-run-examples) - [Examples](#examples) - [gripper\_control](#gripper_control) - [pose\_groupstate](#pose_groupstate) @@ -15,10 +17,12 @@ - [aruco\_detection](#aruco_detection) - [color\_detection](#color_detection) -## 起動方法 +## Setup + CRANE+ V2の起動方法は[crane_plus_examplesのREADME](../crane_plus_examples/README.md)を参照してください。 -## サンプルプログラムを実行する +## How to Run Examples + 準備ができたらPythonによるサンプルプログラムを実行します。 例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 @@ -28,13 +32,12 @@ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' 終了するときは`Ctrl+c`を入力します。 -## Gazeboでサンプルプログラムを実行する場合 - -Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 - -```sh -ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true -``` +> [!NOTE] +> Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 +> +> ```sh +> ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true +> ``` ## Examples @@ -45,16 +48,17 @@ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' - [joint_values](#joint_values) - [pick_and_place](#pick_and_place) -実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 - -```sh -$ ros2 launch crane_plus_examples_py example.launch.py -s -Arguments (pass arguments as ':='): - - 'example': - Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place] - (default: 'gripper_control') -``` +> [!NOTE] +> 実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 +> +> ```sh +> $ ros2 launch crane_plus_examples_py example.launch.py -s +> Arguments (pass arguments as ':='): +> +> 'example': +> Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place] +> (default: 'gripper_control') +> ``` --- @@ -136,16 +140,17 @@ Webカメラ搭載モデルのカメラを使用したサンプルコードで - [aruco\_detection](#aruco_detection) - [color\_detection](#color_detection) -実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 - -```sh -$ ros2 launch crane_plus_examples_py camera_example.launch.py -s -Arguments (pass arguments as ':='): - - 'example': - Set an example executable name: [color_detection] - (default: 'color_detection') -``` +> [!NOTE] +> 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 +> +> ```sh +> $ ros2 launch crane_plus_examples_py camera_example.launch.py -s +> Arguments (pass arguments as ':='): +> +> 'example': +> Set an example executable name: [color_detection] +> (default: 'color_detection') +> ``` --- From 47366b3c066c37375816a9bae1ce3d32a0e9a0ee Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Thu, 21 May 2026 17:29:23 +0900 Subject: [PATCH 106/111] =?UTF-8?q?=E4=B8=8D=E8=A6=81=E3=81=AA=E3=83=89?= =?UTF-8?q?=E3=83=AB=E3=83=9E=E3=83=BC=E3=82=AF=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md index 84a15342..a53696f7 100644 --- a/crane_plus_examples_py/README.md +++ b/crane_plus_examples_py/README.md @@ -144,7 +144,7 @@ Webカメラ搭載モデルのカメラを使用したサンプルコードで > 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 > > ```sh -> $ ros2 launch crane_plus_examples_py camera_example.launch.py -s +> ros2 launch crane_plus_examples_py camera_example.launch.py -s > Arguments (pass arguments as ':='): > > 'example': From ed04e08e972be7ae52ef80349a7e5abee7565bdb Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 22 May 2026 13:19:56 +0900 Subject: [PATCH 107/111] =?UTF-8?q?=E8=87=AA=E5=8B=95=E6=95=B4=E5=BD=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/joint_values.py | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py index 90d54819..50eb7345 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -48,10 +48,15 @@ def main(args=None): ) # 動作速度の調整 - plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 - plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + # Set 0.0 ~ 1.0 + plan_request_params.max_acceleration_scaling_factor = 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) - # armの関節のjoint1〜4を順番に45[deg]ずつ動かす joint_names = [ 'crane_plus_joint1', 'crane_plus_joint2', @@ -60,17 +65,27 @@ def main(args=None): ] target_joint_value = math.radians(45) + # 現在角度をベースに、目標角度を作成する + current_state = arm.get_start_state() + joint_values = current_state.get_joint_group_positions('arm_tcp') + + # jointのリストを辞書型の形式に変換する + joint_values_dict = dict(zip(joint_names, joint_values)) + + # 各関節角度を順番に-45[deg]に動かす for joint_name in joint_names: - arm.set_start_state_to_current_state() + # 対象のjointに目標値を設定する + joint_values_dict[joint_name] = target_joint_value + robot_state.joint_positions = joint_values_dict - joint_values = {joint_name: target_joint_value} - robot_state.joint_positions = joint_values joint_constraint = construct_joint_constraint( robot_state=robot_state, joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm_tcp'), ) + arm.set_start_state_to_current_state() arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + plan_and_execute(crane_plus, arm, logger, single_plan_parameters=plan_request_params) # SRDF内に定義されている'vertical'の姿勢にする From 43631811f31f272ba5505e937a57ebbd832c8e4e Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 22 May 2026 13:40:38 +0900 Subject: [PATCH 108/111] =?UTF-8?q?crane-x7=E3=81=AB=E5=AE=9F=E8=A3=85?= =?UTF-8?q?=E3=82=92=E5=90=88=E3=82=8F=E3=81=9B=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/pick_and_place.py | 50 ++++++++++++------- .../crane_plus_examples_py/utils.py | 8 +-- 2 files changed, 37 insertions(+), 21 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py index d8737a85..ec331674 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -12,23 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. +import copy import math from crane_plus_examples_py.utils import plan_and_execute -from geometry_msgs.msg import PoseStamped + +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion + from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, PlanRequestParameters, ) from moveit_msgs.msg import BoundingVolume, Constraints, OrientationConstraint, PositionConstraint + import rclpy from rclpy.logging import get_logger from scipy.spatial.transform import Rotation from shape_msgs.msg import SolidPrimitive -def set_goal_constraints(x, y, z, roll, pitch, yaw): +def set_goal_constraints(pose): # 位置姿勢の許容誤差 POSITION_TOLERANCE = 0.00001 ORIENTATION_TOLERANCE = 0.0001 @@ -36,15 +40,7 @@ def set_goal_constraints(x, y, z, roll, pitch, yaw): # 目標位置姿勢 target_pose = PoseStamped() target_pose.header.frame_id = 'crane_plus_base' - target_pose.pose.position.x = x - target_pose.pose.position.y = y - target_pose.pose.position.z = z - rotation = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True) - quat = rotation.as_quat() - target_pose.pose.orientation.x = quat[0] - target_pose.pose.orientation.y = quat[1] - target_pose.pose.orientation.z = quat[2] - target_pose.pose.orientation.w = quat[3] + target_pose.pose = pose # 目標位置姿勢の制約設定 goal_constraints = Constraints() @@ -115,8 +111,28 @@ def main(args=None): GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) - + # 物体を持ち上げる高さ + LIFTING_HEIGHT = 0.03 + # 物体の頭上の位置 + gripper_quat = Rotation.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True).as_quat() + gripper_quat_msg = Quaternion( + x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3] + ) + ABOVE_POSE = Pose(position=Point(x=0.0, y=-0.21, z=0.17), orientation=gripper_quat_msg) + # 物体を掴む位置 + gripper_quat = Rotation.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True).as_quat() + gripper_quat_msg = Quaternion( + x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3] + ) + GRASP_POSE = Pose(position=Point(x=0.0, y=-0.1, z=0.02), orientation=gripper_quat_msg) + PRE_AND_POST_GRASP_POSE = copy.deepcopy(GRASP_POSE) + PRE_AND_POST_GRASP_POSE.position.z = LIFTING_HEIGHT # 物体を置く位置 + gripper_quat = Rotation.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True).as_quat() + gripper_quat_msg = Quaternion( + x=gripper_quat[0], y=gripper_quat[1], z=gripper_quat[2], w=gripper_quat[3] + ) + RELEASE_POSE = Pose(position=Point(x=0.25, y=0.0, z=0.06), orientation=gripper_quat_msg) # SRDF内に定義されている'vertical'の姿勢にする arm.set_start_state_to_current_state() @@ -164,7 +180,7 @@ def main(args=None): # 物体の上に腕を伸ばす arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.21, 0.17, 0.0, 90.0, -90.0) + goal_constraints = set_goal_constraints(ABOVE_POSE) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -173,7 +189,7 @@ def main(args=None): single_plan_parameters=arm_plan_request_params, ) arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.1, 0.05, 0.0, 180.0, -90.0) + goal_constraints = set_goal_constraints(PRE_AND_POST_GRASP_POSE) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -184,7 +200,7 @@ def main(args=None): # 掴みに行く arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.1, 0.02, 0.0, 180.0, -90.0) + goal_constraints = set_goal_constraints(GRASP_POSE) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -207,7 +223,7 @@ def main(args=None): # 持ち上げる arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.0, -0.1, 0.05, 0.0, 180.0, -90.0) + goal_constraints = set_goal_constraints(PRE_AND_POST_GRASP_POSE) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, @@ -228,7 +244,7 @@ def main(args=None): # 下ろす arm.set_start_state_to_current_state() - goal_constraints = set_goal_constraints(0.25, 0.0, 0.06, 0.0, 90.0, 0.0) + goal_constraints = set_goal_constraints(RELEASE_POSE) arm.set_goal_state(motion_plan_constraints=[goal_constraints]) plan_and_execute( crane_plus, diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py index 8e363cee..e97753af 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -20,6 +20,7 @@ def plan_and_execute( multi_plan_parameters=None, sleep_time=0.0, ): + # plan to goal logger.info('Planning trajectory') if multi_plan_parameters is not None: plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters) @@ -29,14 +30,13 @@ def plan_and_execute( plan_result = planning_component.plan() # execute the plan - result = None if plan_result: logger.info('Executing plan') robot_trajectory = plan_result.trajectory - result = robot.execute(robot_trajectory, controllers=[]) + execute_result = robot.execute(robot_trajectory, controllers=[]) else: logger.error('Planning failed') - result = False + execute_result = False time.sleep(sleep_time) - return result + return execute_result From 4ad270bc6163fe2c199c67102ab5723620d3df02 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Fri, 22 May 2026 14:23:57 +0900 Subject: [PATCH 109/111] =?UTF-8?q?crane-x7=E3=81=AB=E5=AE=9F=E8=A3=85?= =?UTF-8?q?=E3=82=92=E5=90=88=E3=82=8F=E3=81=9B=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_plus_examples_py/aruco_detection.py | 88 +++++----- .../crane_plus_examples_py/color_detection.py | 161 +++++++++--------- .../pick_and_place_tf.py | 102 ++++++----- 3 files changed, 175 insertions(+), 176 deletions(-) diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py index 3368ce86..b43f77be 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -21,7 +21,7 @@ from rclpy.node import Node from scipy.spatial.transform import Rotation from sensor_msgs.msg import CameraInfo, Image -import tf2_ros +from tf2_ros import TransformBroadcaster class ImageSubscriber(Node): @@ -33,7 +33,7 @@ def __init__(self): self.camera_info_subscription = self.create_subscription( CameraInfo, 'camera_info', self.camera_info_callback, 10 ) - self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) + self.tf_broadcaster = TransformBroadcaster(self) self.camera_info = None @@ -44,47 +44,49 @@ def image_callback(self, msg): cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) - if self.camera_info: - # ArUcoマーカのデータセットを読み込む - # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの - MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) - # マーカID - ids = [] - # 画像座標系上のマーカ頂点位置 - corners = [] - # マーカの検出 - corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) - if ids is None: - return - # マーカの検出数 - n_markers = len(ids) - # カメラパラメータ - CAMERA_MATRIX = np.array(self.camera_info.k).reshape(3, 3) - DIST_COEFFS = np.array(self.camera_info.d).reshape(1, 5) - # マーカ一辺の長さ 0.04 [m] - MARKER_LENGTH = 0.04 - # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 - rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( - corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS - ) - - # マーカの位置姿勢をtfで配信 - for i in range(n_markers): - # tfの配信 - t = TransformStamped() - t.header = msg.header - t.child_frame_id = 'target_' + str(ids[i][0]) - t.transform.translation.x = tvecs[i][0][0] - t.transform.translation.y = tvecs[i][0][1] - t.transform.translation.z = tvecs[i][0][2] - # 回転ベクトルをクォータニオンに変換 - marker_orientation_rot = Rotation.from_rotvec(rvecs[i][0]) - marker_orientation_quat = marker_orientation_rot.as_quat() - t.transform.rotation.x = marker_orientation_quat[0] - t.transform.rotation.y = marker_orientation_quat[1] - t.transform.rotation.z = marker_orientation_quat[2] - t.transform.rotation.w = marker_orientation_quat[3] - self.tf_broadcaster.sendTransform(t) + if not self.camera_info: + return + + # ArUcoマーカのデータセットを読み込む + # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) + # マーカID + ids = [] + # 画像座標系上のマーカ頂点位置 + corners = [] + # マーカの検出 + corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + if ids is None: + return + # マーカの検出数 + n_markers = len(ids) + # カメラパラメータ + CAMERA_MATRIX = np.array(self.camera_info.k).reshape(3, 3) + DIST_COEFFS = np.array(self.camera_info.d).reshape(1, 5) + # マーカ一辺の長さ 0.04 [m] + MARKER_LENGTH = 0.04 + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS + ) + + # マーカの位置姿勢をtfで配信 + for i in range(n_markers): + # tfの配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = 'target_' + str(ids[i][0]) + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトルをクォータニオンに変換 + marker_orientation_rot = Rotation.from_rotvec(rvecs[i][0]) + marker_orientation_quat = marker_orientation_rot.as_quat() + t.transform.rotation.x = marker_orientation_quat[0] + t.transform.rotation.y = marker_orientation_quat[1] + t.transform.rotation.z = marker_orientation_quat[2] + t.transform.rotation.w = marker_orientation_quat[3] + self.tf_broadcaster.sendTransform(t) def camera_info_callback(self, msg): self.camera_info = msg diff --git a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py index f437fbc3..bf36f639 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -19,7 +19,7 @@ import rclpy from rclpy.node import Node from sensor_msgs.msg import CameraInfo, Image -import tf2_ros +from tf2_ros import TransformBroadcaster class ImageSubscriber(Node): @@ -32,88 +32,91 @@ def __init__(self): CameraInfo, 'camera_info', self.camera_info_callback, 10 ) self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) - self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) + self.tf_broadcaster = TransformBroadcaster(self) self.camera_info = None self.bridge = CvBridge() def image_callback(self, msg): - if self.camera_info: - # 赤い物体を検出するようにHSVの範囲を設定 - # 周囲の明るさ等の動作環境に合わせて調整 - LOW_H_1 = 0 - HIGH_H_1 = 10 - LOW_H_2 = 170 - HIGH_H_2 = 179 - LOW_S = 100 - HIGH_S = 255 - LOW_V = 100 - HIGH_V = 255 - - # ウェブカメラの画像を受け取る - cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) - - # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） - cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) - - # 画像の二値化 - img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) - img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) - - # マスク画像の合成 - img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) - - # ノイズ除去の処理 - kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) - - # 穴埋めの処理 - img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) - - # 画像の検出領域におけるモーメントを計算 - moment = cv2.moments(img_thresholded) - d_m01 = moment['m01'] - d_m10 = moment['m10'] - d_area = moment['m00'] - - # 検出した領域のピクセル数が10000より大きい場合 - if d_area > 10000: - # カメラモデル作成 - camera_model = PinholeCameraModel() - - # カメラのパラメータを設定 - camera_model.fromCameraInfo(self.camera_info) - - # 画像座標系における把持対象物の位置（2D） - pixel_x = d_m10 / d_area - pixel_y = d_m01 / d_area - point = (pixel_x, pixel_y) - - # 補正後の画像座標系における把持対象物の位置を取得（2D） - rect_point = camera_model.rectifyPoint(point) - - # カメラ座標系から見た把持対象物の方向（Ray）を取得する - ray = camera_model.projectPixelTo3dRay(rect_point) - - # カメラの高さを0.44[m]として把持対象物の位置を計算 - CAMERA_HEIGHT = 0.46 - object_position = [ - ray[0] * CAMERA_HEIGHT, - ray[1] * CAMERA_HEIGHT, - ray[2] * CAMERA_HEIGHT, - ] - - # 把持対象物の位置をTFに配信 - t = TransformStamped() - t.header = msg.header - t.child_frame_id = 'target_0' - t.transform.translation.x = object_position[0] - t.transform.translation.y = object_position[1] - t.transform.translation.z = object_position[2] - self.tf_broadcaster.sendTransform(t) - - # 閾値による二値化画像を配信 - img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') - self.image_thresholded_publisher.publish(img_thresholded_msg) + # カメラのパラメータを取得してから処理を行う + if not self.camera_info: + return + + # 赤い物体を検出するようにHSVの範囲を設定 + # 周囲の明るさ等の動作環境に合わせて調整 + LOW_H_1 = 0 + HIGH_H_1 = 10 + LOW_H_2 = 170 + HIGH_H_2 = 179 + LOW_S = 100 + HIGH_S = 255 + LOW_V = 100 + HIGH_V = 255 + + # ウェブカメラの画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + + # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) + + # 画像の二値化 + img_mask_1 = cv2.inRange(cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V)) + img_mask_2 = cv2.inRange(cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V)) + + # マスク画像の合成 + img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) + + # ノイズ除去の処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + + # 穴埋めの処理 + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + + # 画像の検出領域におけるモーメントを計算 + moment = cv2.moments(img_thresholded) + d_m01 = moment['m01'] + d_m10 = moment['m10'] + d_area = moment['m00'] + + # 検出した領域のピクセル数が10000より大きい場合 + if d_area > 10000: + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(self.camera_info) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyPoint(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # カメラの高さを0.46[m]として把持対象物の位置を計算 + CAMERA_HEIGHT = 0.46 + object_position = [ + ray[0] * CAMERA_HEIGHT, + ray[1] * CAMERA_HEIGHT, + ray[2] * CAMERA_HEIGHT, + ] + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = 'target_0' + t.transform.translation.x = object_position[0] + t.transform.translation.y = object_position[1] + t.transform.translation.z = object_position[2] + self.tf_broadcaster.sendTransform(t) + + # 閾値による二値化画像を配信 + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') + self.image_thresholded_publisher.publish(img_thresholded_msg) def camera_info_callback(self, msg): self.camera_info = msg diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py index d6b353c8..6dd60bed 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -18,7 +18,6 @@ from geometry_msgs.msg import PoseStamped -# moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( MoveItPy, @@ -35,56 +34,58 @@ import numpy as np import rclpy -from rclpy.logging import get_logger from rclpy.node import Node from scipy.spatial.transform import Rotation from shape_msgs.msg import SolidPrimitive -import tf2_ros -from tf2_ros import TransformListener, TransformStamped +from tf2_ros import TransformException, TransformListener, TransformStamped from tf2_ros.buffer import Buffer class PickAndPlaceTf(Node): - def __init__(self, crane_plus): - super().__init__('pick_and_place_tf_node') - self.logger = get_logger('pick_and_place_tf') + def __init__(self): + super().__init__('pick_and_place_tf') + self.logger = self.get_logger() + + # tf + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) self.tf_past = TransformStamped() - self.crane_plus = crane_plus - self.crane_plus_arm = crane_plus.get_planning_component('arm_tcp') - self.crane_plus_gripper = crane_plus.get_planning_component('gripper') + + # instantiate MoveItPy instance and get planning component + self.crane_plus = MoveItPy(node_name='moveit_py') + self.logger.info('MoveItPy instance created') + + # アーム制御用 planning component + self.arm = self.crane_plus.get_planning_component('arm_tcp') + # グリッパ制御用 planning component + self.gripper = self.crane_plus.get_planning_component('gripper') + # instantiate a RobotState instance using the current robot model - self.robot_model = crane_plus.get_robot_model() - self.robot_state = RobotState(self.robot_model) + self.robot_model = self.crane_plus.get_robot_model() - # planningのパラメータ設定 - # armのパラメータ設定用 self.arm_plan_request_params = PlanRequestParameters( self.crane_plus, 'ompl_rrtc', ) - # Set 0.0 ~ 1.0 - self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 - - # Set 0.0 ~ 1.0 - self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 - - # gripperのパラメータ設定用 self.gripper_plan_request_params = PlanRequestParameters( self.crane_plus, 'ompl_rrtc', ) - # Set 0.0 ~ 1.0 - self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 + # 動作速度の調整 # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 + self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 + + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # SRDFに定義されている'home'の姿勢にする - self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state(configuration_name='home') + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(configuration_name='home') plan_and_execute( self.crane_plus, - self.crane_plus_arm, + self.arm, self.logger, single_plan_parameters=self.arm_plan_request_params, ) @@ -108,27 +109,23 @@ def __init__(self, crane_plus): jointConstraint.weight = 1.0 constraints.joint_constraints.append(jointConstraint) - self.crane_plus_arm.set_path_constraints(constraints) + self.arm.set_path_constraints(constraints) # 待機姿勢 - self._control_arm(0.0, 0.0, 0.3, 0, 0, 0) - - # tf - self.tf_buffer = Buffer() - self.tf_listener = TransformListener(self.tf_buffer, self) + self._control_arm(0.0, 0.0, 0.3, 0, 0, 0.0) # Call on_timer function every second self.timer = self.create_timer(0.5, self.on_timer) def on_timer(self): # target_0のtf位置姿勢を取得 - tf_msg = TransformStamped() try: tf_msg = self.tf_buffer.lookup_transform( 'crane_plus_base', 'target_0', rclpy.time.Time() ) - except tf2_ros.LookupException as ex: - self.get_logger().info(f'Could not transform base_link to target: {ex}') + except TransformException as ex: + self.logger.info(f'Could not transform base_link to target: {ex}') + return now_time = self.get_clock().now() FILTERING_TIME = rclpy.duration.Duration(seconds=2) @@ -156,11 +153,11 @@ def on_timer(self): if tf_diff < DISTANCE_THRESHOLD: # 把持対象が3秒以上停止している場合ピッキング動作開始 if TF_STOP_TIME > STOP_TIME_THRESHOLD: - self._picking(tf_msg) + self._picking(tf_msg.transform.translation) else: self.tf_past = tf_msg - def _picking(self, tf_msg): + def _picking(self, target_position): GRIPPER_DEFAULT = 0.0 GRIPPER_OPEN = math.radians(-30.0) GRIPPER_CLOSE = math.radians(10.0) @@ -169,8 +166,8 @@ def _picking(self, tf_msg): self._control_gripper(GRIPPER_OPEN) # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 - x = tf_msg.transform.translation.x - y = tf_msg.transform.translation.y + x = target_position.x + y = target_position.y theta_rad = math.atan2(y, x) theta_deg = math.degrees(theta_rad) @@ -209,12 +206,13 @@ def _picking(self, tf_msg): # グリッパ制御 def _control_gripper(self, angle): - self.robot_state.set_joint_group_positions('gripper', [angle]) - self.crane_plus_gripper.set_start_state_to_current_state() - self.crane_plus_gripper.set_goal_state(robot_state=self.robot_state) + self.gripper.set_start_state_to_current_state() + robot_state = RobotState(self.robot_model) + robot_state.set_joint_group_positions('gripper', [angle]) + self.gripper.set_goal_state(robot_state=robot_state) plan_and_execute( self.crane_plus, - self.crane_plus_gripper, + self.gripper, self.logger, single_plan_parameters=self.gripper_plan_request_params, ) @@ -268,11 +266,11 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): goal_constraints.position_constraints.append(position_constraint) goal_constraints.orientation_constraints.append(orientation_constraint) - self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state(motion_plan_constraints=[goal_constraints]) + self.arm.set_start_state_to_current_state() + self.arm.set_goal_state(motion_plan_constraints=[goal_constraints]) result = plan_and_execute( self.crane_plus, - self.crane_plus_arm, + self.arm, self.logger, single_plan_parameters=self.arm_plan_request_params, ) @@ -283,17 +281,13 @@ def main(args=None): # ros2の初期化 rclpy.init(args=args) - # MoveItPy初期化 - crane_plus = MoveItPy(node_name='moveit_py') + pick_and_place_tf_node = PickAndPlaceTf() - # node生成 - pick_and_place_tf_node = PickAndPlaceTf(crane_plus) rclpy.spin(pick_and_place_tf_node) - # MoveItPyの終了 - crane_plus.shutdown() - - # rclpyの終了 + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + pick_and_place_tf_node.destroy_node() rclpy.shutdown() From b88bddc86ac5d815f66e74f6daa9683bc4d5eb25 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 25 May 2026 15:54:47 +0900 Subject: [PATCH 110/111] =?UTF-8?q?=E3=83=AA=E3=83=B3=E3=82=AF=E3=81=A8?= =?UTF-8?q?=E6=96=87=E7=AB=A0=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/README.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md index a53696f7..b93d7dea 100644 --- a/crane_plus_examples_py/README.md +++ b/crane_plus_examples_py/README.md @@ -133,9 +133,7 @@ ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place' Webカメラ搭載モデルのカメラを使用したサンプルコードです。 -[「Webカメラ搭載モデルを使用する場合」](#Webカメラ搭載モデルを使用する場合)の手順に従って、 -`demo.launch`を実行している状態で、 -各サンプルを実行できます。 +[crane_plus_examplesのREADME](../crane_plus_examples/README.md)に記載されている「Webカメラ搭載モデルを使用する場合」の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。 - [aruco\_detection](#aruco_detection) - [color\_detection](#color_detection) From 1b0003a8992229e2c335c14b48e3f183844da764 Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Mon, 25 May 2026 15:56:07 +0900 Subject: [PATCH 111/111] =?UTF-8?q?author=E3=82=92=E8=BF=BD=E8=A8=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples_py/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml index e73cd183..5b37710e 100644 --- a/crane_plus_examples_py/package.xml +++ b/crane_plus_examples_py/package.xml @@ -9,6 +9,7 @@ Nozomi Mizoguchi Atsushi Kuwagata + Yusuke Kato rclpy std_msgs