diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index e1463ba..9653730 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -28,9 +28,11 @@ jobs: with: path: ~/.cache/ccache key: ccache - - name: vcs - run: | - vcs import . < abb.repos + # TODO(luca) Remove build from source of UR robots + # This is only temporary until a new Jazzy sync that includes https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/pull/1153/ is made + - name: TEMPORARY build UR from source + run: + git clone -b 2.4.13 https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver - name: rosdep run: | apt update diff --git a/abb.repos b/abb.repos deleted file mode 100644 index ab5d92c..0000000 --- a/abb.repos +++ /dev/null @@ -1,21 +0,0 @@ -repositories: - thirdparty/abb/abb_libegm: - type: git - url: https://github.com/yadunund/abb_libegm.git - version: feature/scara - thirdparty/abb/abb_librws: - type: git - url: https://github.com/ros-industrial/abb_librws.git - version: master - thirdparty/abb/abb_egm_rws_managers: - type: git - url: https://github.com/yadunund/abb_egm_rws_managers.git - version: feature/scara - thirdparty/abb/abb_ros2_msgs: - type: git - url: https://github.com/gbartyzel/abb_ros2_msgs.git - version: rolling - thirdparty/abb/abb_ros2: - type: git - url: https://github.com/yadunund/abb_ros2.git - version: main diff --git a/nexus_integration_tests/config/abb_irb1300_controllers.yaml b/nexus_integration_tests/config/abb_irb1300_controllers.yaml deleted file mode 100644 index ff5e934..0000000 --- a/nexus_integration_tests/config/abb_irb1300_controllers.yaml +++ /dev/null @@ -1,78 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 250 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - -robot_controller_server: - ros__parameters: - managed_controllers: ["joint_state_broadcaster", "joint_trajectory_controller"] - -joint_trajectory_controller: - ros__parameters: - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - - joint_5 - - joint_6 - command_interfaces: - - position - - velocity - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.01 - goal_time: 0.0 - gains: - joint_1: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_2: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_3: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_4: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_5: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_6: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false diff --git a/nexus_integration_tests/config/abb_irb1300_planner_params.yaml b/nexus_integration_tests/config/abb_irb1300_planner_params.yaml deleted file mode 100644 index df67b10..0000000 --- a/nexus_integration_tests/config/abb_irb1300_planner_params.yaml +++ /dev/null @@ -1,58 +0,0 @@ -motion_planner_server: - ros__parameters: - # List of robots to generate motion plans for - manipulators: ["abb_irb1300"] - # The default moveit group for each manipulator - default_group_name: "manipulator" - # Seconds to wait to connect to an action or service server - timeout_duration: 5 - # Configure the planner_server to query motion plans directly from a move_group - # node that is running by initializing a move_group_interface. - # When set to false, the planner_server will initialize planning pipelines - # to directly compute plans but this has not been implemented yet. - use_move_group_interfaces: true - # When planning for multiple robots, set this to true. - use_namespace: false - # Tolerance for goal position. - goal_tolerance: 0.05 - # Maximum seconds to generate a plan. - planning_time: 1.0 - # Maximum number of replanning attempts - replan_attempts: 10 - # If true, the cartesian interpolator will check for collisions. - collision_aware_cartesian_path: false - # The value of the max_step parameter used in cartesian interpolation. - cartesian_max_step: 0.001 - # The value of the jump_threshold parameter used in cartesian interpolation. - cartesian_jump_threshold: 0.0 - # Set true if trajectory should be executed after a plan is generated. - # The execution is a blocking event. - execute_trajectory: false - # The min_x of workspace bounding box. - workspace_min_x: -1.0 - # The min_y of workspace bounding box. - workspace_min_y: -1.0 - # The min_z of workspace bounding box. - workspace_min_z: -1.0 - # The max_x of workspace bounding box. - workspace_max_x: 1.0 - # The max_y of workspace bounding box. - workspace_max_y: 1.0 - # The max_z of workspace bounding box. - workspace_max_z: 1.0 - # The seconds within which the current robot state should be valid. - get_state_wait_seconds: 0.01 - # Namespaced parameters for each robot when use_namespace is true. - # Is important to ensure move_group, robot_state_publisher and - # joint_state broadcaster all have the same namespace. - # Note: If use_namespace is false, pass these parameters directly. - abb_irb1300: - robot_description: "" - robot_description_semantic: "" - group_name: "manipulator" - # Group specific kinematic properties. - manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/nexus_integration_tests/config/abb_irb910sc_controllers.yaml b/nexus_integration_tests/config/abb_irb910sc_controllers.yaml deleted file mode 100644 index 57a05ed..0000000 --- a/nexus_integration_tests/config/abb_irb910sc_controllers.yaml +++ /dev/null @@ -1,79 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 250 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - forward_command_controller_position: - type: forward_command_controller/ForwardCommandController - -robot_controller_server: - ros__parameters: - managed_controllers: [ - "joint_state_broadcaster", - "joint_trajectory_controller", - # "forward_command_controller_position" - ] - -joint_trajectory_controller: - ros__parameters: - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - command_interfaces: - - position - - velocity - state_interfaces: - - position - - velocity - state_publish_rate: 100.0 - action_monitor_rate: 20.0 - allow_partial_joints_goal: false - constraints: - stopped_velocity_tolerance: 0.01 - goal_time: 0.0 - open_loop_control: false - gains: - joint_1: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_2: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_3: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - joint_4: - p: 0.0 - i: 0.0 - d: 0.0 - i_clamp: 0.0 - ff_velocity_scale: 0.0 - normalize_error: false - -forward_command_controller_position: - ros__parameters: - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - interface_name: position diff --git a/nexus_integration_tests/config/abb_irb910sc_planner_params.yaml b/nexus_integration_tests/config/abb_irb910sc_planner_params.yaml deleted file mode 100644 index 44671a4..0000000 --- a/nexus_integration_tests/config/abb_irb910sc_planner_params.yaml +++ /dev/null @@ -1,43 +0,0 @@ -motion_planner_server: - ros__parameters: - # List of robots to generate motion plans for - manipulators: ["abb_irb910sc"] - # The default moveit group for each manipulator - default_group_name: "manipulator" - # Seconds to wait to connect to an action or service server - timeout_duration: 5 - # Configure the planner_server to query motion plans directly from a move_group - # node that is running by initializing a move_group_interface. - # When set to false, the planner_server will initialize planning pipelines - # to directly compute plans but this has not been implemented yet. - use_move_group_interfaces: true - # When planning for multiple robots, set this to true. - use_namespace: false - # Tolerance for goal position. - goal_tolerance: 0.05 - # Maximum seconds to generate a plan. - planning_time: 5.0 - # Maximum number of replanning attempts - replan_attempts: 10 - # If true, the cartesian interpolator will check for collisions. - collision_aware_cartesian_path: false - # Set true if trajectory should be executed after a plan is generated. - # The execution is a blocking event. - execute_trajectory: false - # The seconds within which the current robot state should be valid. - get_state_wait_seconds: 0.01 - - # Namespaced parameters for each robot when use_namespace is true. - # Is important to ensure move_group, robot_state_publisher and - # joint_state broadcaster all have the same namespace. - # Note: If use_namespace is false, pass these parameters directly. - abb_irb910sc: - robot_description: "" - robot_description_semantic: "" - group_name: "manipulator" - # Group specific kinematic properties. - manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 diff --git a/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml b/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml index 7315fa6..650df92 100644 --- a/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml +++ b/nexus_integration_tests/config/workcell_1_bts/place_on_conveyor.xml @@ -5,23 +5,23 @@ - + - + - + - + diff --git a/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml b/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml index 4e2e2f8..0289724 100644 --- a/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml +++ b/nexus_integration_tests/config/workcell_2_bts/pick_from_conveyor.xml @@ -9,25 +9,25 @@ - + - + - + - + - + diff --git a/nexus_integration_tests/launch/launch.py b/nexus_integration_tests/launch/launch.py index f3b11fe..b182dd2 100644 --- a/nexus_integration_tests/launch/launch.py +++ b/nexus_integration_tests/launch/launch.py @@ -126,14 +126,15 @@ def launch_setup(context, *args, **kwargs): "ros_domain_id": str(workcell_1_domain_id), "headless": headless, "use_zenoh_bridge": use_zenoh_bridge, - "controller_config_package": "nexus_integration_tests", - "planner_config_package": "nexus_integration_tests", - "planner_config_file": "abb_irb910sc_planner_params.yaml", - "support_package": "abb_irb910sc_support", - "robot_xacro_file": "irb910sc_3_45.xacro", - "moveit_config_package": "abb_irb910sc_3_45_moveit_config", - "moveit_config_file": "abb_irb910sc_3_45.srdf.xacro", - "controllers_file": "abb_irb910sc_controllers.yaml", + "controller_config_package": "ur_robot_driver", + "planner_config_package": "nexus_motion_planner", + "planner_config_file": "planner_params.yaml", + "support_package": "ur_robot_driver", + "robot_xacro_file": "ur.urdf.xacro", + "moveit_config_package": "ur_moveit_config", + "moveit_config_file": "ur.srdf.xacro", + "controllers_file": "ur_controllers.yaml", + "ur_type": "ur5e", "tf_publisher_launch_file": "workcell_1_tf.launch.py", "sku_detection_params_file": "product_detector_config.yaml", "dispenser_properties": "productA", @@ -168,14 +169,15 @@ def launch_setup(context, *args, **kwargs): "ros_domain_id": str(workcell_2_domain_id), "headless": headless, "use_zenoh_bridge": use_zenoh_bridge, - "controller_config_package": "nexus_integration_tests", - "planner_config_package": "nexus_integration_tests", - "planner_config_file": "abb_irb1300_planner_params.yaml", - "support_package": "abb_irb1300_support", - "robot_xacro_file": "irb1300_10_115.xacro", - "moveit_config_package": "abb_irb1300_10_115_moveit_config", - "moveit_config_file": "abb_irb1300_10_115.srdf.xacro", - "controllers_file": "abb_irb1300_controllers.yaml", + "controller_config_package": "ur_robot_driver", + "planner_config_package": "nexus_motion_planner", + "planner_config_file": "planner_params.yaml", + "support_package": "ur_robot_driver", + "robot_xacro_file": "ur.urdf.xacro", + "moveit_config_package": "ur_moveit_config", + "moveit_config_file": "ur.srdf.xacro", + "controllers_file": "ur_controllers.yaml", + "ur_type": "ur5e", "tf_publisher_launch_file": "workcell_2_tf.launch.py", "sku_detection_params_file": "product_detector_config.yaml", "dispenser_properties": "productB", diff --git a/nexus_integration_tests/launch/workcell.launch.py b/nexus_integration_tests/launch/workcell.launch.py index 3cc1b70..95bca86 100644 --- a/nexus_integration_tests/launch/workcell.launch.py +++ b/nexus_integration_tests/launch/workcell.launch.py @@ -77,6 +77,7 @@ def launch_setup(context, *args, **kwargs): bt_path = LaunchConfiguration("bt_path") ros_domain_id = LaunchConfiguration("ros_domain_id") headless = LaunchConfiguration("headless") + ur_type = LaunchConfiguration("ur_type") controller_config_package = LaunchConfiguration("controller_config_package") planner_config_package = LaunchConfiguration("planner_config_package") planner_config_file = LaunchConfiguration("planner_config_file") @@ -94,6 +95,11 @@ def launch_setup(context, *args, **kwargs): zenoh_config_package = LaunchConfiguration("zenoh_config_package") zenoh_config_filename = LaunchConfiguration("zenoh_config_filename") + controller_path = os.path.join( + get_package_share_directory(controller_config_package.perform(context)), + "config", + controllers_file.perform(context), + ) workcell_id_str = workcell_id.perform(context) mock_dispenser_node = LifecycleNode( @@ -178,7 +184,7 @@ def launch_setup(context, *args, **kwargs): ), Parameter( "robot_name", - "abb_irb1300", + "ur5e", ), Parameter("gripper_max_effort", 0.0), ], @@ -196,20 +202,21 @@ def launch_setup(context, *args, **kwargs): IncludeLaunchDescription( [ PathJoinSubstitution([ - FindPackageShare("abb_bringup"), + FindPackageShare("ur_robot_driver"), 'launch', - 'abb_control.launch.py', + 'ur_control.launch.py', ]) ], launch_arguments=[ + ("ur_type", ur_type), ("runtime_config_package", controller_config_package), - ("controllers_file", controllers_file), - ("description_package", support_package), - ("description_file", robot_xacro_file), + ("controllers_file", controller_path), ("launch_rviz", "false"), - ("moveit_config_package", moveit_config_package), - ("use_fake_hardware", use_fake_hardware), - ("rws_ip", robot_ip), + ("headless_mode", headless), + ("use_mock_hardware", use_fake_hardware), + ("mock_sensor_commands", use_fake_hardware), + ("robot_ip", robot_ip), + ("initial_joint_controller", "joint_trajectory_controller"), ], ) ] @@ -295,9 +302,14 @@ def generate_launch_description(): default_value="true", description="Launch in headless mode (no gui)", ), + DeclareLaunchArgument( + "ur_type", + default_value="ur5e", + description="Type of UR robot (i.e. ur5e, ur10, etc.)", + ), DeclareLaunchArgument( "controller_config_package", - default_value="nexus_integration_tests", + default_value="ur_robot_driver", description="Package with the controller\'s configuration in 'config' folder", ), DeclareLaunchArgument( @@ -312,27 +324,27 @@ def generate_launch_description(): ), DeclareLaunchArgument( "support_package", - default_value="abb_irb910sc_support", + default_value="ur_robot_driver", description="Name of the support package", ), DeclareLaunchArgument( "robot_xacro_file", - default_value="irb910sc_3_45.xacro", + default_value="ur.urdf.xacro", description="Xacro describing the robot.", ), DeclareLaunchArgument( "moveit_config_package", - default_value="abb_irb910sc_3_45_moveit_config", + default_value="ur_moveit_config", description="MoveIt configuration package for the robot", ), DeclareLaunchArgument( "moveit_config_file", - default_value="abb_irb910sc_3_45.srdf.xacro", + default_value="ur.srdf.xacro", description="Name of the SRDF file", ), DeclareLaunchArgument( "controllers_file", - default_value="abb_irb910sc_controllers.yaml", + default_value="ur_controllers.yaml", description="YAML file with the controllers configuration.", ), DeclareLaunchArgument( @@ -342,7 +354,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( "sku_detection_params_file", - default_value="irb910sc_detection.yaml", + default_value="product_detector_config.yaml", description="Config file with the detected SKU poses", ), DeclareLaunchArgument( @@ -353,7 +365,7 @@ def generate_launch_description(): DeclareLaunchArgument( "use_fake_hardware", default_value="True", - description="Set True if running with real hardware.", + description="Set True if simulating without real hardware.", ), DeclareLaunchArgument( "robot_ip", diff --git a/nexus_integration_tests/package.xml b/nexus_integration_tests/package.xml index 2cb79d9..36b616b 100644 --- a/nexus_integration_tests/package.xml +++ b/nexus_integration_tests/package.xml @@ -27,11 +27,9 @@ yaml_cpp_vendor rmw_cyclonedds_cpp - abb_bringup - abb_irb1300_support - abb_irb1300_10_115_moveit_config - abb_irb910sc_support - abb_irb910sc_3_45_moveit_config + ur_robot_driver + ur_description + ur_moveit_config launch launch_ros launch_xml diff --git a/nexus_motion_planner/README.md b/nexus_motion_planner/README.md index 6b48408..0c57bef 100644 --- a/nexus_motion_planner/README.md +++ b/nexus_motion_planner/README.md @@ -36,14 +36,14 @@ Explanation of each configuration parameter are commented in [planner_params.yam ## 2. Launch the motion planner server A demo script that launches `move_group` along with the `motion_planner_server` is included. -By default the `abb_irb1300` robot is used. +By default the `ur5e` robot is used. ```bash ros2 launch nexus_motion_planner demo_planner_server.launch.py ``` To try a different robot, launch with some parameter substitutions. ```bash -ros2 launch nexus_motion_planner demo_planner_server.launch.py support_package:=abb_irb910sc_support robot_xacro_file:=irb910sc_3_45.xacro moveit_config_package:=abb_irb910sc_3_45_moveit_config moveit_config_file:=abb_irb910sc_3_45.srdf.xacro +ros2 launch nexus_motion_planner demo_planner_server.launch.py support_package:=ur_robot_driver robot_xacro_file:=ur.urdf.xacro moveit_config_package:=ur_moveit_config moveit_config_file:=ur.srdf.xacro ``` ## 3. Configuring and activating up the Motion Planner Server lifecycle node @@ -61,34 +61,15 @@ ros2 lifecycle set /motion_planner_server activate Refer to [src/test_request.cpp](src/test_request.cpp) for an example on how to interact with the Motion Planner Server via the [nexus_motion_planner_msgs::GetMotionPlan](nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv) service. Below are some example poses/joint values given for manual testing. -### IRB910SC +### UR5E ```bash # Specifying end-effector poses. Optionally, start value can be specified "-s " -# Home state (frame_id: item): 0.150,-0.200,0.051,1,0,0,0 -# paste_item state (frame_id: item): 0.145,-0.166,-0.053,1.00,0.0,0.0,0.0 -# pick_item state (frame_id: item): -0.384,0.193,0.046,1.00,0.0,0.0,0.0 -ros2 run nexus_motion_planner test_request -name abb_irb910sc -frame_id item -goal_type 0 -t -0.384,0.193,0.046,1.00,0.0,0.0,0.0 - -# Specifying joint values. Optionally, start value can be specified "-sj " -# home state: 0,0,0,0 -# paste_item state: 0,0,-0.1,0 -# pick_item state: 1.2,1.2,-0.1,0.1 -ros2 run nexus_motion_planner test_request -name abb_irb910sc -frame_id item -goal_type 1 -tj 1.2,1.2,-0.1,0.1 -``` - -### IRB1300 -```bash -# Specifying end-effector poses. Optionally, start value can be specified "-s " -# Home state: 0.654250,0.0,1.143500,0.0,0.707107,0.0,0.707107 -# paste_item state: 0.794981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107 -# pick_item state: 0.054310,0.765854,0.677203,-0.498732,0.592116,0.454073,0.441002 -ros2 run nexus_motion_planner test_request -name abb_irb1300 -frame_id item -goal_type 0 -t 0.794981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107 +# test_state state: 0.594981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107 +ros2 run nexus_motion_planner test_request -name ur5e -frame_id item -goal_type 0 -t 0.594981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107 # Specifying joint values. Optionally, start value can be specified "-sj " # home state: 0,0,0,0,0,0 -# paste_item state: 0,1.3,0,0,0,0 -# pick_item state: 1.5,0.5,0.5,0,-0.8,0.1 -ros2 run nexus_motion_planner test_request -name abb_irb1300 -frame_id item -goal_type 1 -tj 1.5,0.5,0.5,0,-0.8,0.1 +ros2 run nexus_motion_planner test_request -name ur5e -frame_id item -goal_type 1 -tj 1.5,0.5,0.5,0,-0.8,0.1 ``` > Note: For debugging purposes, you can also add a `-send_traj` flag which will publish a joint trajectory message on "/manipulator_controller/joint_trajectory". diff --git a/nexus_motion_planner/config/planner_params.yaml b/nexus_motion_planner/config/planner_params.yaml index cfca6a5..3bf70cf 100644 --- a/nexus_motion_planner/config/planner_params.yaml +++ b/nexus_motion_planner/config/planner_params.yaml @@ -1,9 +1,9 @@ motion_planner_server: ros__parameters: # List of robots to generate motion plans for - manipulators: ["abb_irb1300"] + manipulators: ["ur5e"] # The default moveit group for each manipulator - default_group_name: "manipulator" + default_group_name: "ur_manipulator" # Seconds to wait to connect to an action or service server timeout_duration: 5 # Configure the planner_server to query motion plans directly from a move_group @@ -41,4 +41,4 @@ motion_planner_server: # The max_z of workspace bounding box. workspace_max_z: 1.0 # The seconds within which the current robot state should be valid. - get_state_wait_seconds: 0.01 \ No newline at end of file + get_state_wait_seconds: 0.01 diff --git a/nexus_motion_planner/config/planner_params_with_cache.yaml b/nexus_motion_planner/config/planner_params_with_cache.yaml index 976ecb5..122d311 100644 --- a/nexus_motion_planner/config/planner_params_with_cache.yaml +++ b/nexus_motion_planner/config/planner_params_with_cache.yaml @@ -1,9 +1,9 @@ motion_planner_server: ros__parameters: # List of robots to generate motion plans for - manipulators: ["abb_irb1300"] + manipulators: ["ur5e"] # The default moveit group for each manipulator - default_group_name: "manipulator" + default_group_name: "ur_manipulator" # Seconds to wait to connect to an action or service server timeout_duration: 5 # Configure the planner_server to query motion plans directly from a move_group @@ -73,4 +73,4 @@ motion_planner_server: # # Goal tolerances should be more strict so the end point of the robot remains consistent between # the fetched plan and the desired goal. - cache_goal_match_tolerance: 0.001 \ No newline at end of file + cache_goal_match_tolerance: 0.001 diff --git a/nexus_motion_planner/launch/demo_planner_server.launch.py b/nexus_motion_planner/launch/demo_planner_server.launch.py index 79d8a40..87d19ee 100644 --- a/nexus_motion_planner/launch/demo_planner_server.launch.py +++ b/nexus_motion_planner/launch/demo_planner_server.launch.py @@ -14,6 +14,7 @@ import os import yaml +import xacro from ament_index_python.packages import get_package_share_directory @@ -50,6 +51,8 @@ def launch_setup(context, *args, **kwargs): robot_xacro_file = LaunchConfiguration("robot_xacro_file") moveit_config_package = LaunchConfiguration("moveit_config_package") moveit_config_file = LaunchConfiguration("moveit_config_file") + ur_type = LaunchConfiguration("ur_type") + ur_name = LaunchConfiguration("ur_name") planner_server_params = PathJoinSubstitution( [ @@ -66,6 +69,7 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution( [FindPackageShare(support_package), "urdf", robot_xacro_file] ), + " use_mock_hardware:=true name:=", ur_name.perform(context), " ur_type:=", ur_type.perform(context), ] ) robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} @@ -75,16 +79,17 @@ def launch_setup(context, *args, **kwargs): PathJoinSubstitution([FindExecutable(name="xacro")]), " ", PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "config", moveit_config_file] + [FindPackageShare(moveit_config_package), "srdf", moveit_config_file] ), + " name:=", ur_name.perform(context), ] ) robot_description_semantic = { "robot_description_semantic": ParameterValue(robot_description_semantic_content, value_type=str) } - kinematics_yaml = load_yaml( - moveit_config_package.perform(context), "config/kinematics.yaml") + kinematics_yaml = {"robot_description_kinematics": load_yaml( + moveit_config_package.perform(context), "config/kinematics.yaml")} joint_limits_yaml = { "robot_description_planning": load_yaml(moveit_config_package.perform(context), "config/joint_limits.yaml") @@ -233,28 +238,28 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "support_package", - default_value="abb_irb1300_support", + default_value="ur_robot_driver", description="Name of the support package", ) ) declared_arguments.append( DeclareLaunchArgument( "robot_xacro_file", - default_value="irb1300_10_115.xacro", + default_value="ur.urdf.xacro", description="Xacro describing the robot.", ) ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_package", - default_value="abb_irb1300_10_115_moveit_config", + default_value="ur_moveit_config", description="Name of the support package", ) ) declared_arguments.append( DeclareLaunchArgument( "moveit_config_file", - default_value="abb_irb1300_10_115.srdf.xacro", + default_value="ur.srdf.xacro", description="Name of the SRDF file", ) ) @@ -272,6 +277,20 @@ def generate_launch_description(): description="Publish the static transform for item", ) ) + declared_arguments.append( + DeclareLaunchArgument( + "ur_name", + default_value="ur5e", + description="Name of the UR robot", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + default_value="ur5e", + description="UR robot type (i.e. ur5e, ur10, etc.) to be used for xacro generation", + ) + ) return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] ) diff --git a/nexus_motion_planner/package.xml b/nexus_motion_planner/package.xml index 40ef807..6d7db91 100644 --- a/nexus_motion_planner/package.xml +++ b/nexus_motion_planner/package.xml @@ -23,10 +23,8 @@ warehouse_ros_sqlite xacro - abb_irb910sc_3_45_moveit_config - abb_irb910sc_support - abb_irb1300_10_115_moveit_config - abb_irb1300_support + ur_description + ur_robot_driver ament_cmake_pytest ament_cmake_uncrustify diff --git a/nexus_motion_planner/test/test_request.test.py b/nexus_motion_planner/test/test_request.test.py index ab1c51f..5290198 100644 --- a/nexus_motion_planner/test/test_request.test.py +++ b/nexus_motion_planner/test/test_request.test.py @@ -20,6 +20,7 @@ import launch from launch.actions import ( + DeclareLaunchArgument, ExecuteProcess, LogInfo, RegisterEventHandler, @@ -39,6 +40,7 @@ from launch_ros.actions import Node from launch_ros.descriptions import ParameterValue +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare import launch_testing @@ -72,8 +74,7 @@ def set_lifecycle(server_name, transition): output='screen', ) -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) +def load_yaml(package_path, file_path): absolute_file_path = os.path.join(package_path, file_path) try: @@ -99,12 +100,12 @@ def generate_test_description(): proc_env = os.environ.copy() proc_env['PYTHONUNBUFFERED'] = '1' - robot_xacro_file = "irb1300_10_115.xacro" - moveit_config_file = "abb_irb1300_10_115.srdf.xacro" + robot_xacro_file = "ur.urdf.xacro" + moveit_config_file = "ur.srdf.xacro" planner_config_file = "planner_params.yaml" - moveit_config_package = "abb_irb1300_10_115_moveit_config" - support_package = "abb_irb1300_support" + moveit_config_package = get_package_share_directory("ur_moveit_config") + support_package = get_package_share_directory("ur_robot_driver") runtime_config_package = "nexus_motion_planner" planner_server_params = PathJoinSubstitution( @@ -115,29 +116,25 @@ def generate_test_description(): ] ) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(support_package), "urdf", robot_xacro_file] - ), - ] + robot_description_config = xacro.process_file( + os.path.join( + support_package, + "urdf", + robot_xacro_file, + ), + mappings={"ur_type": "ur5e", "name": "ur5e", "use_mock_hardware": "true"}, ) - robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} - - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "config", moveit_config_file] - ), - ] + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_description_semantic_config = xacro.process_file( + os.path.join( + moveit_config_package, + "srdf", + moveit_config_file, + ), + mappings={"name": "ur5e"}, ) - robot_description_semantic = { - "robot_description_semantic": ParameterValue(robot_description_semantic_content, value_type=str) - } + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_config.toxml()} robot_state_publisher = Node( package="robot_state_publisher", @@ -147,8 +144,8 @@ def generate_test_description(): parameters=[robot_description], ) - kinematics_yaml = load_yaml( - moveit_config_package, "config/kinematics.yaml") + kinematics_yaml = {"robot_description_kinematics": load_yaml( + moveit_config_package, "config/kinematics.yaml")} joint_limits_yaml = { "robot_description_planning": load_yaml(moveit_config_package, "config/joint_limits.yaml") @@ -194,20 +191,20 @@ def generate_test_description(): # Load ros2_control controller (so joint states are published) # This needs to be loaded in tandem with the MoveIt controller manager ros2_controllers_path = os.path.join( - get_package_share_directory(moveit_config_package), + support_package, "config", - "ros2_controllers.yaml", + "ur_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], + parameters=[robot_description, ParameterFile(ros2_controllers_path, allow_substs=True)], output="log", ) load_controllers = [] for controller in [ - "manipulator_controller", + "joint_trajectory_controller", "joint_state_broadcaster" ]: load_controllers += [ @@ -270,14 +267,14 @@ def generate_test_description(): # Send get motion plan requests with specified start and target # pose values - get_plan_irb1300_poses = Node( + get_plan_ur5e_poses = Node( package='nexus_motion_planner', executable='test_request', arguments=[ - "-name", "abb_irb1300", + "-name", "ur5e", "-frame_id", "item", "-goal_type", "0", - "-t", "0.794981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107", + "-t", "0.594981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107", ], output='both', ) @@ -329,14 +326,14 @@ def generate_test_description(): target_action=lifecycle_set_configure2, on_exit=[lifecycle_set_activate2])) - trigger_get_plan_irb1300_poses = RegisterEventHandler( + trigger_get_plan_ur5e_poses = RegisterEventHandler( OnProcessExit( target_action=lifecycle_set_activate2, on_exit=[ LogInfo(msg='Sending motion planning task with end-effector poses'), TimerAction( period=5.0, - actions=[get_plan_irb1300_poses] + actions=[get_plan_ur5e_poses] ) ] ) @@ -344,10 +341,19 @@ def generate_test_description(): node_mapping = { 'motion_planner': motion_planner, - 'get_plan_irb1300_poses': get_plan_irb1300_poses, + 'get_plan_ur5e_poses': get_plan_ur5e_poses, } + # Only used because ur_description expects it + tf_prefix_argument = DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="tf_prefix of the joint names, useful for " + "multi-robot setup. If changed, also joint names in the controllers' configuration " + "have to be updated.") + return launch.LaunchDescription([ + tf_prefix_argument, robot_state_publisher, motion_planner, move_group_node, @@ -359,7 +365,7 @@ def generate_test_description(): trigger_lifecycle_4, trigger_lifecycle_5, trigger_lifecycle_6, - trigger_get_plan_irb1300_poses, + trigger_get_plan_ur5e_poses, ros2_control_node, *load_controllers, launch_testing.actions.ReadyToTest() @@ -369,7 +375,7 @@ class TestGetMotionPlanService(unittest.TestCase): def test_read_task_client_stderr( self, proc_output, - get_plan_irb1300_poses): + get_plan_ur5e_poses): """Check if task client has sent service request and received successful response""" get_plan_poses_str = [ @@ -380,7 +386,7 @@ def test_read_task_client_stderr( for expected_output in get_plan_poses_str: proc_output.assertWaitFor( expected_output, - process=get_plan_irb1300_poses, timeout=40, stream='stderr') + process=get_plan_ur5e_poses, timeout=40, stream='stderr') def test_read_server_stderr( self, @@ -399,17 +405,17 @@ def test_read_server_stderr( process=motion_planner, timeout=60, stream='stderr') class TestWait(unittest.TestCase): - def test_wait_for_get_plan_clients(self, proc_info, get_plan_irb1300_poses): + def test_wait_for_get_plan_clients(self, proc_info, get_plan_ur5e_poses): """Wait until process ends""" - proc_info.assertWaitForShutdown(process=get_plan_irb1300_poses, timeout=40) + proc_info.assertWaitForShutdown(process=get_plan_ur5e_poses, timeout=40) @launch_testing.post_shutdown_test() class TestShutdown(unittest.TestCase): def test_read_task_client_stderr( self, proc_info, - get_plan_irb1300_poses): + get_plan_ur5e_poses): """Check if the processes exited normally.""" launch_testing.asserts.assertExitCodes(proc_info, [0], - process=get_plan_irb1300_poses) + process=get_plan_ur5e_poses) diff --git a/nexus_motion_planner/test/test_request_with_cache.test.py b/nexus_motion_planner/test/test_request_with_cache.test.py index 98f31fd..4a0021a 100644 --- a/nexus_motion_planner/test/test_request_with_cache.test.py +++ b/nexus_motion_planner/test/test_request_with_cache.test.py @@ -20,6 +20,7 @@ import launch from launch.actions import ( + DeclareLaunchArgument, ExecuteProcess, LogInfo, RegisterEventHandler, @@ -39,6 +40,7 @@ from launch_ros.actions import Node from launch_ros.descriptions import ParameterValue +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare import launch_testing @@ -72,8 +74,7 @@ def set_lifecycle(server_name, transition): output='screen', ) -def load_yaml(package_name, file_path): - package_path = get_package_share_directory(package_name) +def load_yaml(package_path, file_path): absolute_file_path = os.path.join(package_path, file_path) try: @@ -99,12 +100,12 @@ def generate_test_description(): proc_env = os.environ.copy() proc_env['PYTHONUNBUFFERED'] = '1' - robot_xacro_file = "irb1300_10_115.xacro" - moveit_config_file = "abb_irb1300_10_115.srdf.xacro" + robot_xacro_file = "ur.urdf.xacro" + moveit_config_file = "ur.srdf.xacro" planner_config_file = "planner_params_with_cache.yaml" - moveit_config_package = "abb_irb1300_10_115_moveit_config" - support_package = "abb_irb1300_support" + moveit_config_package = get_package_share_directory("ur_moveit_config") + support_package = get_package_share_directory("ur_robot_driver") runtime_config_package = "nexus_motion_planner" planner_server_params = PathJoinSubstitution( @@ -115,29 +116,25 @@ def generate_test_description(): ] ) - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(support_package), "urdf", robot_xacro_file] - ), - ] + robot_description_config = xacro.process_file( + os.path.join( + support_package, + "urdf", + robot_xacro_file, + ), + mappings={"ur_type": "ur5e", "name": "ur5e", "use_mock_hardware": "true"}, ) - robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} - - robot_description_semantic_content = Command( - [ - PathJoinSubstitution([FindExecutable(name="xacro")]), - " ", - PathJoinSubstitution( - [FindPackageShare(moveit_config_package), "config", moveit_config_file] - ), - ] + robot_description = {"robot_description": robot_description_config.toxml()} + + robot_description_semantic_config = xacro.process_file( + os.path.join( + moveit_config_package, + "srdf", + moveit_config_file, + ), + mappings={"name": "ur5e"}, ) - robot_description_semantic = { - "robot_description_semantic": ParameterValue(robot_description_semantic_content, value_type=str) - } + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_config.toxml()} robot_state_publisher = Node( package="robot_state_publisher", @@ -147,8 +144,8 @@ def generate_test_description(): parameters=[robot_description], ) - kinematics_yaml = load_yaml( - moveit_config_package, "config/kinematics.yaml") + kinematics_yaml = {"robot_description_kinematics": load_yaml( + moveit_config_package, "config/kinematics.yaml")} joint_limits_yaml = { "robot_description_planning": load_yaml(moveit_config_package, "config/joint_limits.yaml") @@ -194,20 +191,20 @@ def generate_test_description(): # Load ros2_control controller (so joint states are published) # This needs to be loaded in tandem with the MoveIt controller manager ros2_controllers_path = os.path.join( - get_package_share_directory(moveit_config_package), + support_package, "config", - "ros2_controllers.yaml", + "ur_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", executable="ros2_control_node", - parameters=[robot_description, ros2_controllers_path], + parameters=[robot_description, ParameterFile(ros2_controllers_path, allow_substs=True)], output="log", ) load_controllers = [] for controller in [ - "manipulator_controller", + "joint_trajectory_controller", "joint_state_broadcaster" ]: load_controllers += [ @@ -270,14 +267,14 @@ def generate_test_description(): # Send get motion plan requests with specified start and target # pose values - get_plan_irb1300_poses = Node( + get_plan_ur5e_poses = Node( package='nexus_motion_planner', executable='test_request', arguments=[ - "-name", "abb_irb1300", + "-name", "ur5e", "-frame_id", "item", "-goal_type", "0", - "-t", "0.794981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107", + "-t", "0.594981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107", ], output='both', ) @@ -329,14 +326,14 @@ def generate_test_description(): target_action=lifecycle_set_configure2, on_exit=[lifecycle_set_activate2])) - trigger_get_plan_irb1300_poses = RegisterEventHandler( + trigger_get_plan_ur5e_poses = RegisterEventHandler( OnProcessExit( target_action=lifecycle_set_activate2, on_exit=[ LogInfo(msg='Sending motion planning task with end-effector poses'), TimerAction( period=5.0, - actions=[get_plan_irb1300_poses] + actions=[get_plan_ur5e_poses] ) ] ) @@ -344,10 +341,19 @@ def generate_test_description(): node_mapping = { 'motion_planner': motion_planner, - 'get_plan_irb1300_poses': get_plan_irb1300_poses, + 'get_plan_ur5e_poses': get_plan_ur5e_poses, } + # Only used because ur_description expects it + tf_prefix_argument = DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="tf_prefix of the joint names, useful for " + "multi-robot setup. If changed, also joint names in the controllers' configuration " + "have to be updated.") + return launch.LaunchDescription([ + tf_prefix_argument, robot_state_publisher, motion_planner, move_group_node, @@ -359,7 +365,7 @@ def generate_test_description(): trigger_lifecycle_4, trigger_lifecycle_5, trigger_lifecycle_6, - trigger_get_plan_irb1300_poses, + trigger_get_plan_ur5e_poses, ros2_control_node, *load_controllers, launch_testing.actions.ReadyToTest() @@ -369,7 +375,7 @@ class TestGetMotionPlanService(unittest.TestCase): def test_read_task_client_stderr( self, proc_output, - get_plan_irb1300_poses): + get_plan_ur5e_poses): """Check if task client has sent service request and received successful response""" get_plan_poses_str = [ @@ -380,7 +386,7 @@ def test_read_task_client_stderr( for expected_output in get_plan_poses_str: proc_output.assertWaitFor( expected_output, - process=get_plan_irb1300_poses, timeout=40, stream='stderr') + process=get_plan_ur5e_poses, timeout=40, stream='stderr') def test_read_server_stderr( self, @@ -400,17 +406,17 @@ def test_read_server_stderr( process=motion_planner, timeout=60, stream='stderr') class TestWait(unittest.TestCase): - def test_wait_for_get_plan_clients(self, proc_info, get_plan_irb1300_poses): + def test_wait_for_get_plan_clients(self, proc_info, get_plan_ur5e_poses): """Wait until process ends""" - proc_info.assertWaitForShutdown(process=get_plan_irb1300_poses, timeout=40) + proc_info.assertWaitForShutdown(process=get_plan_ur5e_poses, timeout=40) @launch_testing.post_shutdown_test() class TestShutdown(unittest.TestCase): def test_read_task_client_stderr( self, proc_info, - get_plan_irb1300_poses): + get_plan_ur5e_poses): """Check if the processes exited normally.""" launch_testing.asserts.assertExitCodes(proc_info, [0], - process=get_plan_irb1300_poses) + process=get_plan_ur5e_poses) diff --git a/nexus_robot_controller/CMakeLists.txt b/nexus_robot_controller/CMakeLists.txt index ede0773..e73d507 100644 --- a/nexus_robot_controller/CMakeLists.txt +++ b/nexus_robot_controller/CMakeLists.txt @@ -63,7 +63,7 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake) install(DIRECTORY test DESTINATION share/${PROJECT_NAME}) - add_launch_test(test/robot_controller_abb_irb910sc.test.py) + add_launch_test(test/robot_controller_ur5e.test.py) endif() ament_export_include_directories(include) diff --git a/nexus_robot_controller/config/test_abb_irb910sc_controllers.yaml b/nexus_robot_controller/config/test_abb_irb910sc_controllers.yaml deleted file mode 100644 index c359883..0000000 --- a/nexus_robot_controller/config/test_abb_irb910sc_controllers.yaml +++ /dev/null @@ -1,42 +0,0 @@ -controller_manager: - ros__parameters: - update_rate: 250 # Hz - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - - forward_command_controller_position: - type: forward_command_controller/ForwardCommandController - -joint_trajectory_controller: - ros__parameters: - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - command_interfaces: - - position - - velocity - state_interfaces: - - position - - velocity - 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 - open_loop_control: false - -forward_command_controller_position: - ros__parameters: - joints: - - joint_1 - - joint_2 - - joint_3 - - joint_4 - interface_name: position diff --git a/nexus_robot_controller/config/test_ur5e_controllers.yaml b/nexus_robot_controller/config/test_ur5e_controllers.yaml new file mode 100644 index 0000000..826d152 --- /dev/null +++ b/nexus_robot_controller/config/test_ur5e_controllers.yaml @@ -0,0 +1,18 @@ +controller_manager: + ros__parameters: + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: position_controllers/JointGroupPositionController + + +forward_position_controller: + ros__parameters: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/nexus_robot_controller/package.xml b/nexus_robot_controller/package.xml index 1cc0957..bbaa162 100644 --- a/nexus_robot_controller/package.xml +++ b/nexus_robot_controller/package.xml @@ -31,7 +31,7 @@ std_msgs xacro - abb_irb910sc_support + ur_robot_driver ament_cmake_uncrustify rmf_utils ros2_control diff --git a/nexus_robot_controller/src/robot_controller_server.cpp b/nexus_robot_controller/src/robot_controller_server.cpp index 05f02c5..8277e56 100644 --- a/nexus_robot_controller/src/robot_controller_server.cpp +++ b/nexus_robot_controller/src/robot_controller_server.cpp @@ -71,6 +71,7 @@ RobotControllerServer::RobotControllerServer( { declare_parameter("robot_description", ""); declare_parameter("managed_controllers", std::vector()); + declare_parameter("ros2_control_params_file", std::string()); pimpl_->server_logging_prefix_ << "[CONTROLLER SERVER: " << pimpl_->ns_ << "/" << node_name << "] "; @@ -193,6 +194,9 @@ RobotControllerServer::on_configure(const rclcpp_lifecycle::State& /*state*/) node_->get_parameter("managed_controllers", managed_controllers); for (auto& controller_name : managed_controllers) { + std::string params_file; + node_->get_parameter("ros2_control_params_file", params_file); + pimpl_->cm_node_->set_parameter({controller_name + ".params_file", params_file}); pimpl_->cm_node_->load_controller(controller_name); pimpl_->cm_node_->configure_controller(controller_name); } diff --git a/nexus_robot_controller/test/abb_irb910sc_test_params.yaml b/nexus_robot_controller/test/abb_irb910sc_test_params.yaml deleted file mode 100644 index 7b2844d..0000000 --- a/nexus_robot_controller/test/abb_irb910sc_test_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -robot_controller_server: - ros__parameters: - managed_controllers: ["joint_state_broadcaster", "forward_command_controller_position"] diff --git a/nexus_robot_controller/test/robot_controller_abb_irb910sc.test.py b/nexus_robot_controller/test/robot_controller_ur5e.test.py similarity index 84% rename from nexus_robot_controller/test/robot_controller_abb_irb910sc.test.py rename to nexus_robot_controller/test/robot_controller_ur5e.test.py index c3e1253..ee706c8 100644 --- a/nexus_robot_controller/test/robot_controller_abb_irb910sc.test.py +++ b/nexus_robot_controller/test/robot_controller_ur5e.test.py @@ -71,13 +71,14 @@ def load_yaml(package_name, file_path): @pytest.mark.launch_test @launch_testing.markers.keep_alive def generate_test_description(): - robot_description_path = get_package_share_directory("abb_irb910sc_support") + robot_description_path = get_package_share_directory("ur_robot_driver") robot_description_config = xacro.process_file( os.path.join( robot_description_path, "urdf", - "irb910sc_3_45.xacro", - ) + "ur.urdf.xacro", + ), + mappings={"ur_type": "ur5e", "name": "ur5e", "use_mock_hardware": "true"}, ) robot_description = {"robot_description": robot_description_config.toxml()} @@ -103,7 +104,7 @@ def generate_test_description(): name="rviz2", arguments=[ "-d", - os.path.join(robot_description_path, "rviz", "urdf_description.rviz"), + os.path.join(robot_description_path, "rviz", "view_robot.rviz"), ], output="log", condition=IfCondition(start_rviz), @@ -111,11 +112,15 @@ def generate_test_description(): robot_controller_path = get_package_share_directory("nexus_robot_controller") robot_controller_test_params = PathJoinSubstitution( - [robot_controller_path, "test", "abb_irb910sc_test_params.yaml"] + [robot_controller_path, "test", "ur5e_test_params.yaml"] ) controller_params = PathJoinSubstitution( - [robot_controller_path, "config", "test_abb_irb910sc_controllers.yaml"] + [robot_controller_path, "config", "test_ur5e_controllers.yaml"] + ) + ros2_control_params = PathJoinSubstitution( + [robot_controller_path, "config", "ur5e_forward_position_controller.yaml"] ) + controller_params_file = {"ros2_control_params_file": controller_params} remappings = [] return LaunchDescription(declared_arguments + [ @@ -126,7 +131,7 @@ def generate_test_description(): package='nexus_robot_controller', executable='robot_controller_server', output='screen', - parameters=[robot_controller_test_params, controller_params, robot_description], + parameters=[robot_controller_test_params, controller_params, controller_params_file, robot_description], remappings=remappings ), @@ -155,8 +160,8 @@ def test_robot_controller_server(self, launch_service, proc_info, proc_output): proc_info.assertWaitForStartup(process=echo_joint_states, timeout=5) os.system('ros2 topic pub -1 ' - 'forward_command_controller_position/commands std_msgs/msg/Float64MultiArray ' - '"{data: [-5.0, -5.0, -5.0, -5.0]}"') + 'forward_position_controller/commands std_msgs/msg/Float64MultiArray ' + '"{data: [-5.0, -5.0, -5.0, -5.0, -5.0, -5.0]}"') proc_output.assertWaitFor('- -5.', timeout=10, stream='stdout') @@ -169,14 +174,14 @@ def test_robot_controller_server(self, launch_service, proc_info, proc_output): proc_output.assertWaitFor('Switched on Controllers', timeout=10, stream='stderr') os.system('ros2 topic pub -1 ' - 'forward_command_controller_position/commands std_msgs/msg/Float64MultiArray ' - '"{data: [5.0, 5.0, 5.0, 5.0]}"') + 'forward_position_controller/commands std_msgs/msg/Float64MultiArray ' + '"{data: [5.0, 5.0, 5.0, 5.0, 5.0, 5.0]}"') proc_output.assertWaitFor('- 5.', timeout=10, stream='stdout') os.system('ros2 topic pub -1 ' - 'forward_command_controller_position/commands std_msgs/msg/Float64MultiArray ' - '"{data: [0.0, 0.0, 0.0, 0.0]}"') + 'forward_position_controller/commands std_msgs/msg/Float64MultiArray ' + '"{data: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}"') os.system('ros2 lifecycle set /robot_controller_server deactivate') proc_output.assertWaitFor('Deactivating', timeout=10, stream='stderr') diff --git a/nexus_robot_controller/test/ur5e_test_params.yaml b/nexus_robot_controller/test/ur5e_test_params.yaml new file mode 100644 index 0000000..bae3ff3 --- /dev/null +++ b/nexus_robot_controller/test/ur5e_test_params.yaml @@ -0,0 +1,3 @@ +robot_controller_server: + ros__parameters: + managed_controllers: ["joint_state_broadcaster", "forward_position_controller"]