diff --git a/gh_pages/_source/session3/ros2/3-Build-a-MoveIt-Package.md b/gh_pages/_source/session3/ros2/3-Build-a-MoveIt-Package.md index cb3753513..32d37892e 100644 --- a/gh_pages/_source/session3/ros2/3-Build-a-MoveIt-Package.md +++ b/gh_pages/_source/session3/ros2/3-Build-a-MoveIt-Package.md @@ -1,10 +1,9 @@ # Build a MoveIt! Package >In this exercise, we will create a MoveIt! package for an industrial robot. This package creates the configuration and launch files required to use a robot with the MoveIt! Motion-Control nodes. In general, the MoveIt! package does not contain any C++ code. -> **_IMPORTANT: This exercise requires a mix of ROS1 and ROS2 environments. Be careful what ROS environment is sourced in each terminal. The "MoveIt Setup Assistant" is not yet available in ROS2, so we run the ROS1 version. But it outputs a ROS1-compatible package, so we need to adapt the output for ROS2._** ## Motivation -MoveIt! is a free-space motion planning framework for ROS. It’s an incredibly useful and easy-to-use tool for planning motions between two points in space without colliding with anything. Under the hood MoveIt is quite complicated, but (unlike most ROS libraries) it has a really nice GUI Wizard to get you started. +MoveIt! is a free-space motion planning framework for ROS. It’s an incredibly useful and easy-to-use tool for planning motions between two points in space without colliding with anything. Under the hood MoveIt is quite complicated, but (unlike most ROS libraries) it has a nice GUI Wizard to get you started. ## Reference Example [Using MoveIt with ROS-I](http://wiki.ros.org/Industrial/Tutorials/Create_a_MoveIt_Pkg_for_an_Industrial_Robot) @@ -23,32 +22,33 @@ In this exercise, you will generate a MoveIt package for the UR5 workcell you bu ### Create a Base Package using the Setup Assistant -1. Open a NEW terminal and setup your ROS1 workspace to run the MoveIt Setup Assistant. Put copies of the required URDF packages (`ur_description`, `myworkcell_support`) inside this ROS1 workspace, to make them visible to the Setup Assistant: +1. Open a NEW terminal and set up your ROS1 workspace to run the MoveIt Setup Assistant. Put copies of the required URDF packages (`ur_description`, `myworkcell_support`) inside this ROS1 workspace, to make them visible to the Setup Assistant: ``` - cd ~/ros1_ws/src + cd ~/ros2_ws/src git clone https://github.com/ros-industrial/universal_robot.git - cp -r ~/industrial_training/exercises/3.3/ros1/src/myworkcell_support ~/ros1_ws/src + cp -r ~/industrial_training/exercises/3.3/ros1/src/myworkcell_support ~/ros2_ws/src - + - cd ~/ros1_ws/ - source /opt/ros/noetic/setup.bash + cd ~/ros2_ws/ + source /opt/ros/humble/setup.bash catkin build - source ~/ros1_ws/devel/setup.bash + source ~/ros2_ws/devel/setup.bash ``` 1. Start the MoveIt! Setup Assistant (don't forget auto-complete with tab): ``` - roslaunch moveit_setup_assistant setup_assistant.launch + ros2 launch moveit_setup_assistant setup_assistant.launch.py ``` - 1. Select "Create New MoveIt Configuration Package", select the `workcell.xacro` in ROS1 workspace's _myworkcell_support_ package, then "Load File". + 1. Select "Create New MoveIt Configuration Package", and select the `workcell.urdf.xacro` in the workspace's _myworkcell_support_ package, then "Load File". 1. Work your way through the tabs on the left from the top down. - 1. Generate a self-collision matrix. + 1. In "Self-Collisions", move the slider "Sampling Density" to the highest possible value and generate a self-collision matrix. After a few seconds the MoveIt! Setup Assistant will present you the results of the computation. + 1. Add a fixed virtual base joint. e.g. ``` @@ -61,15 +61,26 @@ In this exercise, you will generate a MoveIt package for the UR5 workcell you bu 1. Add a planning group called `manipulator` that names the kinematic chain between `base_link` and `tool0`. Note: Follow [ROS naming guidelines/requirements](http://wiki.ros.org/ROS/Patterns/Conventions) and don't use any whitespace, anywhere. a. Set the kinematics solver to `KDLKinematicsPlugin` - a. Set the default OMPL planner to `RRTConnect` - 1. Create a few named positions (e.g. "home", "allZeros", etc.) to test with motion-planning. + b. Set the default OMPL planner to `RRTConnect` + + c. Using "Add Kin. Chain" the Base Link and Tip Link can be specified as `base_link` and `tool0` + + d. after saving the planning group, select `Joints` and `Èdit Selected`, add `shoulder_pan_joint`, `shoulder_lift_joint`, `elbow_joint` and `wrist_[1-3]_joint` and save again. + + + 1. In `Robot Poses`, create a few named positions (e.g. "home", "allZeros", etc.) to test with motion-planning. 1. Don't worry about adding end effectors/grippers or passive joints for this exercise. + + 1. Skip the ros2_control URDF Model pane, as we use the ros2_control model provided by UR Robot Driver. - 1. In the "ROS Controllers" tab, use the "Auto Add FollowJointsTrajectory" button to define a basic _FollowJointTrajectory_ controller for the entire UR5 arm. + 1. In the "ROS 2 Controllers" tab, use the "Auto Add JointTrajectoryController Controllers For Each Planning Group" button to define a basic _joint_trajectory_controller_ for the entire UR5 arm. Rename the `manipulator_controller` to `joint_trajectory_controller` + + 1. In the "Moveit Controllers" tab, use the "Auto Add FollowJointsTrajectory Controllers For Each Planning Group" button to define a basic _follow_joint_controller_ for the entire UR5 arm. Rename the `manipulator_controller` to `joint_trajectory_controller` + - 1. Skip the Simulation and 3D Perception tabs. + 1. Skip the Simulation and Launch Files tabs. 1. Enter author / maintainer info. @@ -80,259 +91,320 @@ In this exercise, you will generate a MoveIt package for the UR5 workcell you bu ## Create ROS2 Package from Setup Assistant Output - The outcome of the Setup Assistant is a new ROS1 package that contains a large number of launch and configuration files. We need to add a few files to customize this general-purpose package for our application and convert it to work with ROS2, where the setup assistant is not yet available. - - > _Note: The world of MoveIt is continually evolving in ROS2; this is currently just one way to get a ROS2 package going and is not presented as the "correct" way._ - - 1. Create a new empty package inside your **ROS2** workspace: - - ``` - source ~/ros2_ws/install/setup.bash - cd ~/ros2_ws/src - ros2 pkg create myworkcell_moveit_config --dependencies myworkcell_support - ``` - - 1. Create an empty `config/` subdirectory inside the new package and copy the following files from the `config/` subdirectory of the ROS1 MoveIt config package: - - ``` - myworkcell.srdf - joint_limits.yaml - kinematics.yaml - ompl_planning.yaml - ``` - - 1. Open the ROS2 `config/ompl_planning.yaml` file in a text editor and add the following lines at the bottom: - - ``` - 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 - ``` - - 1. Create a new `controllers.yaml` file in the ROS2 MoveIt package's `config` directory: + The outcome of the Setup Assistant is a new ROS1 package that contains a large number of launch and configuration files. We need to add a few files to customize this general-purpose package for our application. - ``` - controller_names: - - manipulator_joint_trajectory_controller - - manipulator_joint_trajectory_controller: - action_ns: follow_joint_trajectory - type: FollowJointTrajectory - default: true - joints: - - shoulder_pan_joint - - shoulder_lift_joint - - elbow_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - ``` + > _Note: The world of MoveIt is continually evolving in ROS2; this is currently just one way to get a ROS2 package going and is not presented as the "correct" way._ - This file will configure MoveIt to use a controller for joint trajectory execution provided by `ros_control`. - 1. Create a new `ros_controllers.yaml` file in the ROS2 MoveIt package's `config` directory: - ``` - controller_manager: - ros__parameters: - update_rate: 600 # Hz - manipulator_joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - joint_state_controller: - type: joint_state_controller/JointStateController - - # parameters for each controller listed under controller manager - manipulator_joint_trajectory_controller: - ros__parameters: - command_interfaces: - - position - state_interfaces: - - position - - velocity - joints: - - elbow_joint - - shoulder_lift_joint - - shoulder_pan_joint - - wrist_1_joint - - wrist_2_joint - - wrist_3_joint - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.0 - goal_time: 0.0 - - joint_state_controller: - ros__parameters: - type: joint_state_controller/JointStateController - ``` +* You need to set up cthe orrect initial positions for the robot. To do so overwrite the content of the file `initial_positions.yaml` with the following lines: +``` +initial_positions: + elbow_joint: -1.57 + shoulder_lift_joint: -1.57 + shoulder_pan_joint: -1.57 + wrist_1_joint: -1.57 + wrist_2_joint: 1.57 + wrist_3_joint: 0.0 + +elbow_joint: -1.57 +shoulder_lift_joint: -1.57 +shoulder_pan_joint: -1.57 +wrist_1_joint: -1.57 +wrist_2_joint: 1.57 +wrist_3_joint: 0.0 +``` + + +* Next you need to setup the correct joint limits. To do so overwrite your `joint_limits.yaml` with: + +``` +# 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: + elbow_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_lift_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + shoulder_pan_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + wrist_1_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + wrist_2_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + wrist_3_joint: + has_velocity_limits: true + max_velocity: 3.1415926535897931 + has_acceleration_limits: false + max_acceleration: 0 + +``` + +* Then we need to fix the ROS2 controllers configuration. The UR robot has many more controllers than the ones Moveit knows about. Therefore copy the following contents into your `ros2_controllers.yaml`. + +``` +controller_manager: + ros__parameters: + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + io_and_status_controller: + type: ur_controllers/GPIOController + + speed_scaling_state_broadcaster: + type: ur_controllers/SpeedScalingStateBroadcaster + + force_torque_sensor_broadcaster: + type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + scaled_joint_trajectory_controller: + type: ur_controllers/ScaledJointTrajectoryController + + forward_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + forward_position_controller: + type: position_controllers/JointGroupPositionController + + update_rate: 500 # Hz + + +speed_scaling_state_broadcaster: + ros__parameters: + state_publish_rate: 100.0 + + +force_torque_sensor_broadcaster: + ros__parameters: + sensor_name: tcp_fts_sensor + state_interface_names: + - force.x + - force.y + - force.z + - torque.x + - torque.y + - torque.z + frame_id: tool0 + topic_name: ft_data + + +joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + + +scaled_joint_trajectory_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 100.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + elbow_joint: { trajectory: 0.2, goal: 0.1 } + wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + +forward_velocity_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + interface_name: velocity + +forward_position_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +``` + +* Finally we need to add the configuration for the ompl planner. To do so add ``ompl_planning.yaml`` to your config folder with the following lines: + +``` +planning_plugin: ompl_interface/OMPLPlanner +request_adapters: >- + default_planner_request_adapters/AddTimeOptimalParameterization + default_planner_request_adapters/ResolveConstraintFrames + 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 +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 expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar +arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 + +``` +* The UR robot comes with a quite complex xacro file that needs many arguments which **Moveit Setup Assistant** does not know about. Replace the contents of myworkcell.urdf.xacro with the following content. + +```xml + + + + + + + + + + + + + + + + +``` These parameters configure the controller nodes at startup. - 1. Create a new `launch` directory inside the ROS2 moveit package, and a new launch file, `myworkcell_planning_execution.launch.py` inside that directory. This launch file will act as a single location to start up all components needed for both motion planning and execution. + Rename the file `demo.launch.py`in the launch folder to `myworkcell_planning_execution.launch.py` - ``` - import os - import yaml - import xacro - from launch import LaunchDescription - from launch_ros.actions import Node - from ament_index_python import get_package_share_directory - - def get_package_file(package, file_path): - """Get the location of a file installed in an ament package""" - package_path = get_package_share_directory(package) - absolute_file_path = os.path.join(package_path, file_path) - return absolute_file_path - - def load_file(file_path): - """Load the contents of a file into a string""" - try: - with open(file_path, 'r') as file: - return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - def load_yaml(file_path): - """Load a yaml file into a dictionary""" - try: - with open(file_path, 'r') as file: - return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available - return None - - def run_xacro(xacro_file): - """Run xacro and output a file in the same directory with the same name, w/o a .xacro suffix""" - urdf_file, ext = os.path.splitext(xacro_file) - if ext != '.xacro': - raise RuntimeError(f'Input file to xacro must have a .xacro extension, got {xacro_file}') - os.system(f'xacro {xacro_file} -o {urdf_file}') - return urdf_file - - - def generate_launch_description(): - xacro_file = get_package_file('myworkcell_support', 'urdf/workcell.urdf.xacro') - urdf_file = run_xacro(xacro_file) - srdf_file = get_package_file('myworkcell_moveit_config', 'config/myworkcell.srdf') - kinematics_file = get_package_file('myworkcell_moveit_config', 'config/kinematics.yaml') - ompl_config_file = get_package_file('myworkcell_moveit_config', 'config/ompl_planning.yaml') - moveit_controllers_file = get_package_file('myworkcell_moveit_config', 'config/controllers.yaml') - ros_controllers_file = get_package_file('myworkcell_moveit_config', 'config/ros_controllers.yaml') - - robot_description = load_file(urdf_file) - robot_description_semantic = load_file(srdf_file) - kinematics_config = load_yaml(kinematics_file) - ompl_config = load_yaml(ompl_config_file) - - moveit_controllers = { - 'moveit_simple_controller_manager' : load_yaml(moveit_controllers_file), - '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.01 - } - planning_scene_monitor_config = { - 'publish_planning_scene': True, - 'publish_geometry_updates': True, - 'publish_state_updates': True, - 'publish_transforms_updates': True - } - - # MoveIt node - move_group_node = Node( - package='moveit_ros_move_group', - executable='move_group', - output='screen', - parameters=[ - { - 'robot_description': robot_description, - 'robot_description_semantic': robot_description_semantic, - 'robot_description_kinematics': kinematics_config, - 'ompl': ompl_config, - 'planning_pipelines': ['ompl'], - }, - moveit_controllers, - trajectory_execution, - planning_scene_monitor_config, - ], - ) - # TF information - robot_state_publisher = Node( - name='robot_state_publisher', - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[ - {'robot_description': robot_description} - ] - ) - # Visualization (parameters needed for MoveIt display plugin) - rviz = Node( - name='rviz', - package='rviz2', - executable='rviz2', - output='screen', - parameters=[ - { - 'robot_description': robot_description, - 'robot_description_semantic': robot_description_semantic, - 'robot_description_kinematics': kinematics_config, - } - ], - ) - # Controller manager for realtime interactions - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters= [ - {'robot_description': robot_description}, - ros_controllers_file - ], - output="screen", - ) - # Startup up ROS2 controllers (will exit immediately) - controller_names = ['manipulator_joint_trajectory_controller', 'joint_state_controller'] - spawn_controllers = [ - Node( - package="controller_manager", - executable="spawner.py", - arguments=[controller], - output="screen") - for controller in controller_names - ] - - return LaunchDescription([ - move_group_node, - robot_state_publisher, - ros2_control_node, - rviz, - ] + spawn_controllers - ) - ``` - - The bulk of the complexity here is finding and loading all the required parameters using the same helper functions we've used in previous launch files. This launch file defines four nodes to start with the most important one being the `move_group` node. Note that RViz is also started as node, which allows us to initialize it with the same parameters as the move_group node. - - 1. Open the new package's `CMakeLists.txt` file and add an installation rule for the `config/` and `launch/` directories underneath the calls to `find_package`: + 1. Rebuild the workspace (`colcon build`) and now you are ready to make use of the newly created MoveIt! Package. - ``` - install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME}) - ``` - - 1. Rebuild the workspace (`colcon build`) and test a launch file to see if the new package loads without errors: - - ``` - source ~/ros2_ws/install/setup.bash - ros2 launch myworkcell_moveit_config myworkcell_planning_execution.launch.py - ``` + You can try out your newly created MoveIt! package by launching the following: + `ros2 launch myworkcell_moveit_config myworkcell_planning_execution.launch` + RViz should start and the UR5 should be visualized. > _Don't worry too much about how to use RViz. We'll work through that in the next exercise._