diff --git a/.github/workflows/industrial_ci.yml b/.github/workflows/industrial_ci.yml index 95c0d087..5bf1c933 100644 --- a/.github/workflows/industrial_ci.yml +++ b/.github/workflows/industrial_ci.yml @@ -17,9 +17,9 @@ jobs: strategy: matrix: env: - - { ROS_DISTRO: humble, ROS_REPO: ros } + - { ROS_DISTRO: jazzy, ROS_REPO: ros } runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - uses: "ros-industrial/industrial_ci@master" env: ${{ matrix.env }} diff --git a/README.en.md b/README.en.md index 930de8a5..e31cd22d 100644 --- a/README.en.md +++ b/README.en.md @@ -13,7 +13,7 @@ ROS 2 package suite of CRANE-X7. - [crane\_x7\_ros](#crane_x7_ros) - [Table of Contents](#table-of-contents) - [Supported ROS 2 distributions](#supported-ros-2-distributions) - - [ROS](#ros) + - [ROS 1](#ros-1) - [Requirements](#requirements) - [Installation](#installation) - [Build from source](#build-from-source) @@ -24,35 +24,34 @@ ROS 2 package suite of CRANE-X7. ## Supported ROS 2 distributions - [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel) -- Humble +- [Humble](https://github.com/rt-net/crane_x7_ros/tree/humble) +- [Jazzy](https://github.com/rt-net/crane_x7_ros/tree/jazzy) -### ROS +### ROS 1 - [Melodic](https://github.com/rt-net/crane_x7_ros/tree/master) - [Noetic](https://github.com/rt-net/crane_x7_ros/tree/master) + ## Requirements - CRANE-X7 - [Product Introduction](https://rt-net.jp/products/crane-x7/) - [Web Shop](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3660&language=en) - Linux OS - - Ubuntu 22.04 + - Ubuntu 24.04 - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) + - [Jazzy](https://docs.ros.org/en/jazzy/Installation.html) ## Installation ### Build from source ```sh -# Setup ROS environment -$ source /opt/ros/humble/setup.bash - # Download crane_x7 repositories $ mkdir -p ~/ros2_ws/src $ cd ~/ros2_ws/src -$ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git -$ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git +$ git clone -b $ROS_DISTRO https://github.com/rt-net/crane_x7_ros.git +$ git clone -b $ROS_DISTRO https://github.com/rt-net/crane_x7_description.git # Install dependencies $ rosdep install -r -y -i --from-paths . diff --git a/README.md b/README.md index 0b1f4e3d..5a4ba829 100644 --- a/README.md +++ b/README.md @@ -13,7 +13,7 @@ ROS 2 package suite of CRANE-X7. - [crane\_x7\_ros](#crane_x7_ros) - [Table of Contents](#table-of-contents) - [Supported ROS 2 distributions](#supported-ros-2-distributions) - - [ROS](#ros) + - [ROS 1](#ros-1) - [Requirements](#requirements) - [Installation](#installation) - [Build from source](#build-from-source) @@ -25,9 +25,10 @@ ROS 2 package suite of CRANE-X7. ## Supported ROS 2 distributions - [Foxy](https://github.com/rt-net/crane_x7_ros/tree/foxy-devel) -- Humble +- [Humble](https://github.com/rt-net/crane_x7_ros/tree/humble) +- [Jazzy](https://github.com/rt-net/crane_x7_ros/tree/jazzy) -### ROS +### ROS 1 - [Melodic](https://github.com/rt-net/crane_x7_ros/tree/master) - [Noetic](https://github.com/rt-net/crane_x7_ros/tree/master) @@ -38,23 +39,20 @@ ROS 2 package suite of CRANE-X7. - [製品ページ](https://rt-net.jp/products/crane-x7/) - [ウェブショップ](https://www.rt-shop.jp/index.php?main_page=product_info&products_id=3660) - Linux OS - - Ubuntu 22.04 + - Ubuntu 24.04 - ROS - - [Humble Hawksbill](https://docs.ros.org/en/humble/Installation.html) + - [Jazzy](https://docs.ros.org/en/jazzy/Installation.html) ## Installation ### Build from source ```sh -# Setup ROS environment -$ source /opt/ros/humble/setup.bash - # Download crane_x7 repositories $ mkdir -p ~/ros2_ws/src $ cd ~/ros2_ws/src -$ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git -$ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git +$ git clone -b $ROS_DISTRO https://github.com/rt-net/crane_x7_ros.git +$ git clone -b $ROS_DISTRO https://github.com/rt-net/crane_x7_description.git # Install dependencies $ rosdep install -r -y -i --from-paths . diff --git a/crane_x7_examples/launch/camera_example.launch.py b/crane_x7_examples/launch/camera_example.launch.py index 03a25fe1..1500b927 100644 --- a/crane_x7_examples/launch/camera_example.launch.py +++ b/crane_x7_examples/launch/camera_example.launch.py @@ -19,8 +19,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import SetParameter from launch_ros.actions import Node +from launch_ros.actions import SetParameter import yaml @@ -67,7 +67,7 @@ def generate_launch_description(): description=('Set true when using the gazebo simulator.') ) - picking_node = Node(name="pick_and_place_tf", + picking_node = Node(name='pick_and_place_tf', package='crane_x7_examples', executable='pick_and_place_tf', output='screen', diff --git a/crane_x7_examples/launch/demo.launch.py b/crane_x7_examples/launch/demo.launch.py index c75c8557..9b7cead4 100644 --- a/crane_x7_examples/launch/demo.launch.py +++ b/crane_x7_examples/launch/demo.launch.py @@ -56,6 +56,24 @@ def generate_launch_description(): 'manipulator_links.csv' ) + declare_rviz_config = DeclareLaunchArgument( + 'rviz_config', + default_value=get_package_share_directory( + 'crane_x7_moveit_config' + ) + '/config/moveit.rviz', + description='Set the path to rviz configuration file.', + condition=UnlessCondition(LaunchConfiguration('use_d435')), + ) + + declare_rviz_config_camera = DeclareLaunchArgument( + 'rviz_config', + default_value=get_package_share_directory( + 'crane_x7_examples' + ) + '/launch/camera_example.rviz', + description='Set the path to rviz configuration file.', + condition=IfCondition(LaunchConfiguration('use_d435')), + ) + description_loader = RobotDescriptionLoader() description_loader.port_name = LaunchConfiguration('port_name') description_loader.baudrate = LaunchConfiguration('baudrate') @@ -70,22 +88,9 @@ def generate_launch_description(): PythonLaunchDescriptionSource([ get_package_share_directory('crane_x7_moveit_config'), '/launch/run_move_group.launch.py']), - condition=UnlessCondition(LaunchConfiguration('use_d435')), - launch_arguments={ - 'loaded_description': description - }.items() - ) - - rviz_config_file = get_package_share_directory( - 'crane_x7_examples') + '/launch/camera_example.rviz' - move_group_camera = IncludeLaunchDescription( - PythonLaunchDescriptionSource([ - get_package_share_directory('crane_x7_moveit_config'), - '/launch/run_move_group.launch.py']), - condition=IfCondition(LaunchConfiguration('use_d435')), launch_arguments={ 'loaded_description': description, - 'rviz_config_file': rviz_config_file + 'rviz_config': LaunchConfiguration('rviz_config') }.items() ) @@ -113,8 +118,9 @@ def generate_launch_description(): declare_port_name, declare_baudrate, declare_use_d435, + declare_rviz_config, + declare_rviz_config_camera, move_group, - move_group_camera, control_node, realsense_node ]) diff --git a/crane_x7_examples/launch/example.launch.py b/crane_x7_examples/launch/example.launch.py index 15b71bf4..6e8bc635 100644 --- a/crane_x7_examples/launch/example.launch.py +++ b/crane_x7_examples/launch/example.launch.py @@ -18,9 +18,9 @@ from crane_x7_description.robot_description_loader import RobotDescriptionLoader from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch_ros.actions import SetParameter from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node +from launch_ros.actions import SetParameter import yaml diff --git a/crane_x7_examples/src/aruco_detection.cpp b/crane_x7_examples/src/aruco_detection.cpp index e32b7b9f..0c14168f 100644 --- a/crane_x7_examples/src/aruco_detection.cpp +++ b/crane_x7_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_x7_examples/src/cartesian_path.cpp b/crane_x7_examples/src/cartesian_path.cpp index de6d07d9..1807a999 100644 --- a/crane_x7_examples/src/cartesian_path.cpp +++ b/crane_x7_examples/src/cartesian_path.cpp @@ -23,7 +23,7 @@ #include "angles/angles.h" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -90,11 +90,8 @@ int main(int argc, char ** argv) } moveit_msgs::msg::RobotTrajectory trajectory; - const double jump_threshold = 0.0; const double eef_step = 0.01; - move_group_arm.computeCartesianPath( - waypoints, eef_step, jump_threshold, - trajectory); + move_group_arm.computeCartesianPath(waypoints, eef_step, trajectory); move_group_arm.execute(trajectory); // SRDFに定義されている"home"の姿勢にする diff --git a/crane_x7_examples/src/color_detection.cpp b/crane_x7_examples/src/color_detection.cpp index 72bc9e17..bd1e853c 100644 --- a/crane_x7_examples/src/color_detection.cpp +++ b/crane_x7_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 diff --git a/crane_x7_examples/src/gripper_control.cpp b/crane_x7_examples/src/gripper_control.cpp index b5594bd9..d3a2161b 100644 --- a/crane_x7_examples/src/gripper_control.cpp +++ b/crane_x7_examples/src/gripper_control.cpp @@ -19,7 +19,7 @@ #include #include "angles/angles.h" -#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; diff --git a/crane_x7_examples/src/joint_values.cpp b/crane_x7_examples/src/joint_values.cpp index f2575360..e1d180aa 100644 --- a/crane_x7_examples/src/joint_values.cpp +++ b/crane_x7_examples/src/joint_values.cpp @@ -18,7 +18,7 @@ // /src/move_group_interface_tutorial.cpp #include "angles/angles.h" -#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; diff --git a/crane_x7_examples/src/pick_and_place.cpp b/crane_x7_examples/src/pick_and_place.cpp index ad987bff..f6095b1e 100644 --- a/crane_x7_examples/src/pick_and_place.cpp +++ b/crane_x7_examples/src/pick_and_place.cpp @@ -22,7 +22,7 @@ #include "angles/angles.h" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" -#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" diff --git a/crane_x7_examples/src/pick_and_place_tf.cpp b/crane_x7_examples/src/pick_and_place_tf.cpp index 82c0fe83..8bfbceb1 100644 --- a/crane_x7_examples/src/pick_and_place_tf.cpp +++ b/crane_x7_examples/src/pick_and_place_tf.cpp @@ -28,7 +28,7 @@ #include "geometry_msgs/msg/quaternion.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2/convert.h" diff --git a/crane_x7_examples/src/pose_groupstate.cpp b/crane_x7_examples/src/pose_groupstate.cpp index 490bfd67..6ed4b751 100644 --- a/crane_x7_examples/src/pose_groupstate.cpp +++ b/crane_x7_examples/src/pose_groupstate.cpp @@ -17,7 +17,7 @@ // /5c15da709e9ea8529b54b313dc570f164f9a713e/doc/examples/subframes // /src/subframes_tutorial.cpp -#include "moveit/move_group_interface/move_group_interface.h" +#include "moveit/move_group_interface/move_group_interface.hpp" #include "rclcpp/rclcpp.hpp" using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; diff --git a/crane_x7_gazebo/gui/gui.config b/crane_x7_gazebo/gui/gui.config index 0d9558d0..3d479808 100644 --- a/crane_x7_gazebo/gui/gui.config +++ b/crane_x7_gazebo/gui/gui.config @@ -28,102 +28,102 @@ - + 3D View false docked - + ogre2 scene 0.4 0.4 0.4 0.8 0.8 0.8 - 1.0 0 2.0 0 0.5 3.14 + 1.0 0 1.7 0 0.4 3.14 + 50 - + floating 5 5 false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + World control false false 72 - 121 1 floating @@ -131,7 +131,7 @@ - + true true @@ -142,7 +142,7 @@ - + World stats false false @@ -155,7 +155,7 @@ - + true true @@ -165,7 +165,7 @@ - + false 0 0 @@ -174,12 +174,12 @@ floating false #666666 - + - + false 250 0 @@ -188,12 +188,12 @@ floating false #666666 - + - + false 0 50 @@ -202,15 +202,12 @@ floating false #777777 - - - - false + - + false 250 50 @@ -219,12 +216,12 @@ floating false #777777 - + - + false 300 50 @@ -233,21 +230,21 @@ floating false #777777 - + - + false docked - + - + false docked - + diff --git a/crane_x7_gazebo/launch/crane_x7_with_table.launch.py b/crane_x7_gazebo/launch/crane_x7_with_table.launch.py index f68e0570..4effda42 100644 --- a/crane_x7_gazebo/launch/crane_x7_with_table.launch.py +++ b/crane_x7_gazebo/launch/crane_x7_with_table.launch.py @@ -26,22 +26,22 @@ def generate_launch_description(): # PATHを追加で通さないとSTLファイルが読み込まれない - env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], - 'IGN_GAZEBO_RESOURCE_PATH': os.path.dirname( + env = {'GZ_SIM_SYSTEM_PLUGIN_PATH': os.environ['LD_LIBRARY_PATH'], + 'GZ_SIM_RESOURCE_PATH': os.path.dirname( get_package_share_directory('crane_x7_description'))} world_file = os.path.join( get_package_share_directory('crane_x7_gazebo'), 'worlds', 'table.sdf') gui_config = os.path.join( get_package_share_directory('crane_x7_gazebo'), 'gui', 'gui.config') # -r オプションで起動時にシミュレーションをスタートしないと、コントローラが起動しない - ign_gazebo = ExecuteProcess( - cmd=['ign gazebo -r', world_file, '--gui-config', gui_config], + gz_sim = ExecuteProcess( + cmd=['gz sim -r', world_file, '--gui-config', gui_config], output='screen', additional_env=env, shell=True ) - ignition_spawn_entity = Node( + gz_sim_spawn_entity = Node( package='ros_gz_sim', executable='create', output='screen', @@ -65,19 +65,28 @@ def generate_launch_description(): ) spawn_joint_state_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner joint_state_controller'], + cmd=[ + 'ros2 run controller_manager spawner ' + 'joint_state_controller' + ], shell=True, output='screen', ) spawn_arm_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner crane_x7_arm_controller'], + cmd=[ + 'ros2 run controller_manager spawner ' + 'crane_x7_arm_controller' + ], shell=True, output='screen', ) spawn_gripper_controller = ExecuteProcess( - cmd=['ros2 run controller_manager spawner crane_x7_gripper_controller'], + cmd=[ + 'ros2 run controller_manager spawner ' + 'crane_x7_gripper_controller' + ], shell=True, output='screen', ) @@ -85,14 +94,16 @@ def generate_launch_description(): bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', - arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + arguments=[ + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock', + ], output='screen' ) return LaunchDescription([ SetParameter(name='use_sim_time', value=True), - ign_gazebo, - ignition_spawn_entity, + gz_sim, + gz_sim_spawn_entity, move_group, spawn_joint_state_controller, spawn_arm_controller, diff --git a/crane_x7_gazebo/worlds/table.sdf b/crane_x7_gazebo/worlds/table.sdf index fc20fb94..0ac47520 100644 --- a/crane_x7_gazebo/worlds/table.sdf +++ b/crane_x7_gazebo/worlds/table.sdf @@ -6,40 +6,40 @@ 1.0 + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-user-commands-system" + name="gz::sim::systems::UserCommands"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground%20Plane + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground%20Plane - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Table + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Table 0.29 0 0 0 0 1.5708 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Wood%20cube%205cm + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Wood%20cube%205cm 0.20 0 1.0 0 0 0 diff --git a/crane_x7_moveit_config/.setup_assistant b/crane_x7_moveit_config/.setup_assistant new file mode 100644 index 00000000..6561fcf7 --- /dev/null +++ b/crane_x7_moveit_config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: crane_x7_description + relative_path: urdf/crane_x7.urdf.xacro + srdf: + relative_path: config/crane_x7.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_x7_moveit_config/CMakeLists.txt b/crane_x7_moveit_config/CMakeLists.txt index 568710b7..64bae52e 100644 --- a/crane_x7_moveit_config/CMakeLists.txt +++ b/crane_x7_moveit_config/CMakeLists.txt @@ -3,11 +3,10 @@ project(crane_x7_moveit_config) find_package(ament_cmake REQUIRED) -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) diff --git a/crane_x7_moveit_config/config/controllers.yaml b/crane_x7_moveit_config/config/controllers.yaml deleted file mode 100644 index e5c8c323..00000000 --- a/crane_x7_moveit_config/config/controllers.yaml +++ /dev/null @@ -1,23 +0,0 @@ -controller_names: - - crane_x7_arm_controller - - crane_x7_gripper_controller - -crane_x7_arm_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - crane_x7_shoulder_fixed_part_pan_joint - - crane_x7_shoulder_revolute_part_tilt_joint - - crane_x7_upper_arm_revolute_part_twist_joint - - crane_x7_upper_arm_revolute_part_rotate_joint - - crane_x7_lower_arm_fixed_part_joint - - crane_x7_lower_arm_revolute_part_joint - - crane_x7_wrist_joint - -crane_x7_gripper_controller: - action_ns: gripper_cmd - type: GripperCommand - default: true - joints: - - crane_x7_gripper_finger_a_joint \ No newline at end of file diff --git a/crane_x7_moveit_config/config/kinematics.yaml b/crane_x7_moveit_config/config/kinematics.yaml index 46a23863..05573e5c 100644 --- a/crane_x7_moveit_config/config/kinematics.yaml +++ b/crane_x7_moveit_config/config/kinematics.yaml @@ -1,4 +1,4 @@ arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/crane_x7_moveit_config/launch/run_move_group.rviz b/crane_x7_moveit_config/config/moveit.rviz similarity index 100% rename from crane_x7_moveit_config/launch/run_move_group.rviz rename to crane_x7_moveit_config/config/moveit.rviz diff --git a/crane_x7_moveit_config/config/moveit_controllers.yaml b/crane_x7_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 00000000..58c44026 --- /dev/null +++ b/crane_x7_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,27 @@ +# MoveIt uses this configuration for controller management +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - crane_x7_arm_controller + - crane_x7_gripper_controller + + crane_x7_arm_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - crane_x7_shoulder_fixed_part_pan_joint + - crane_x7_shoulder_revolute_part_tilt_joint + - crane_x7_upper_arm_revolute_part_twist_joint + - crane_x7_upper_arm_revolute_part_rotate_joint + - crane_x7_lower_arm_fixed_part_joint + - crane_x7_lower_arm_revolute_part_joint + - crane_x7_wrist_joint + + crane_x7_gripper_controller: + action_ns: gripper_cmd + type: GripperCommand + default: true + joints: + - crane_x7_gripper_finger_a_joint \ No newline at end of file diff --git a/crane_x7_moveit_config/config/ompl_planning.yaml b/crane_x7_moveit_config/config/ompl_planning.yaml index f98332c4..f4204450 100644 --- a/crane_x7_moveit_config/config/ompl_planning.yaml +++ b/crane_x7_moveit_config/config/ompl_planning.yaml @@ -1,4 +1,25 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +# To optionally use Ruckig for jerk-limited smoothing, add this line to the request adapters below +# default_planning_request_adapters/AddRuckigTrajectorySmoothing +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + planner_configs: + APSConfigDefault: + type: geometric::AnytimePathShortening + shortcut: 1 # Attempt to shortcut all new solution paths + hybridize: 1 # Compute hybrid solution trajectories + max_hybrid_paths: 36 # Number of hybrid paths generated per iteration + num_planners: 8 # The number of default planners to use for planning + planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" SBLkConfigDefault: type: geometric::SBL range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() @@ -20,7 +41,7 @@ planner_configs: 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 + 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 @@ -44,7 +65,7 @@ planner_configs: 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() + 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: @@ -75,9 +96,9 @@ planner_configs: 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 + 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 + 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 @@ -88,13 +109,13 @@ planner_configs: 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_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 + 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 @@ -102,7 +123,7 @@ planner_configs: 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 + 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() @@ -120,8 +141,12 @@ planner_configs: 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: + - APSConfigDefault - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault @@ -145,8 +170,11 @@ arm: - LazyPRMstarkConfigDefault - SPARSkConfigDefault - SPARStwokConfigDefault + - TrajOptDefault + gripper: planner_configs: + - APSConfigDefault - SBLkConfigDefault - ESTkConfigDefault - LBKPIECEkConfigDefault @@ -170,5 +198,7 @@ gripper: - LazyPRMstarkConfigDefault - SPARSkConfigDefault - SPARStwokConfigDefault + - TrajOptDefault + projection_evaluator: joints(crane_x7_gripper_finger_a_joint,crane_x7_gripper_finger_b_joint) longest_valid_segment_fraction: 0.005 \ No newline at end of file diff --git a/crane_x7_moveit_config/config/pilz_cartesian_limits.yaml b/crane_x7_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/crane_x7_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_x7_moveit_config/launch/run_move_group.launch.py b/crane_x7_moveit_config/launch/run_move_group.launch.py index 001246af..615663dd 100644 --- a/crane_x7_moveit_config/launch/run_move_group.launch.py +++ b/crane_x7_moveit_config/launch/run_move_group.launch.py @@ -1,4 +1,4 @@ -# Copyright 2022 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. @@ -12,146 +12,56 @@ # 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_x7_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 -import yaml - -# Reference: https://github.com/ros-planning/moveit2_tutorials/blob/humble/doc/ -# examples/move_group_interface/launch/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 +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_rsp_launch +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch def generate_launch_description(): - description_loader = RobotDescriptionLoader() + ld = LaunchDescription() - declare_loaded_description = DeclareLaunchArgument( - 'loaded_description', - default_value=description_loader.load(), - description='Set robot_description text. \ - It is recommended to use RobotDescriptionLoader() in crane_x7_description.' - ) + description_loader = RobotDescriptionLoader() - declare_rviz_config_file = DeclareLaunchArgument( - 'rviz_config_file', - default_value=get_package_share_directory( - 'crane_x7_moveit_config') + '/launch/run_move_group.rviz', - description='Set the path to rviz configuration file.' + ld.add_action( + DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() in \ + crane_x7_description.', + ) ) - robot_description = {'robot_description': LaunchConfiguration('loaded_description')} - - robot_description_semantic_config = load_file( - 'crane_x7_moveit_config', 'config/crane_x7.srdf') - robot_description_semantic = { - 'robot_description_semantic': robot_description_semantic_config} - - joint_limits_yaml = load_yaml( - "crane_x7_moveit_config", "config/joint_limits.yaml" + moveit_config = ( + MoveItConfigsBuilder('crane_x7') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .planning_pipelines(pipelines=['ompl']) + .to_moveit_configs() ) - robot_description_planning = { - "robot_description_planning": joint_limits_yaml} - - kinematics_yaml = load_yaml('crane_x7_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_x7_moveit_config', 'config/ompl_planning.yaml') - ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) - - # Trajectory Execution Functionality - controllers_yaml = load_yaml('crane_x7_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} + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } - # 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, - robot_description_planning, - kinematics_yaml, - ompl_planning_pipeline_config, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters]) + # Move group + ld.add_entity(generate_move_group_launch(moveit_config)) # 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]) + ld.add_entity(generate_moveit_rviz_launch(moveit_config)) # 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_entity(generate_static_virtual_joint_tfs_launch(moveit_config)) # Publish TF - robot_state_publisher = Node(package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - output='both', - parameters=[robot_description]) + ld.add_entity(generate_rsp_launch(moveit_config)) - return LaunchDescription([declare_loaded_description, - declare_rviz_config_file, - run_move_group_node, - rviz_node, - static_tf, - robot_state_publisher, - ]) + return ld diff --git a/crane_x7_moveit_config/launch/setup_assistant.launch.py b/crane_x7_moveit_config/launch/setup_assistant.launch.py new file mode 100755 index 00000000..007d389a --- /dev/null +++ b/crane_x7_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,24 @@ +# Copyright 2024 RT Corporation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder( + 'crane_x7', package_name='crane_x7_moveit_config' + ).to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/crane_x7_moveit_config/package.xml b/crane_x7_moveit_config/package.xml index 372c33f5..62e72863 100644 --- a/crane_x7_moveit_config/package.xml +++ b/crane_x7_moveit_config/package.xml @@ -9,14 +9,28 @@ Shota Aoki Atsushi Kuwagata + Nozomi Mizoguchi ament_cmake crane_x7_description - moveit - robot_state_publisher - rviz2 - tf2_ros + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + controller_manager + 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 ament_lint_auto ament_lint_common