Add warehouse support

UniversalRobots · May 13, 2024 · 3e59ee7 · 3e59ee7
1 parent a500aff
commit 3e59ee7
Showing 1 changed file with 45 additions and 19 deletions.
diff --git a/ur_moveit_config/launch/ur_moveit.launch.py b/ur_moveit_config/launch/ur_moveit.launch.py
@@ -28,6 +28,8 @@
 
 #
 # Author: Felix Exner
+import os
+
 from pathlib import Path
 
 from launch import LaunchDescription
@@ -42,9 +44,6 @@
 from launch_ros.substitutions import FindPackageShare
 
 from moveit_configs_utils import MoveItConfigsBuilder
-from moveit_configs_utils.launches import (
-    generate_move_group_launch,
-)
 
 
 def declare_arguments():
@@ -66,39 +65,66 @@ def declare_arguments():
                     "ur30",
                 ],
             ),
+            DeclareLaunchArgument(
+                "warehouse_sqlite_path",
+                default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
+                description="Path where the warehouse database should be stored",
+            ),
         ]
     )
 
 
 def generate_launch_description():
     launch_rviz = LaunchConfiguration("launch_rviz")
     ur_type = LaunchConfiguration("ur_type")
+    warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
 
     moveit_config = (
         MoveItConfigsBuilder(robot_name="ur", package_name="ur_moveit_config")
         .robot_description_semantic(Path("srdf") / "ur.srdf.xacro", {"name": ur_type})
         .to_moveit_configs()
     )
 
+    warehouse_ros_config = {
+        "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
+        "warehouse_host": warehouse_sqlite_path,
+    }
+
+    ld = LaunchDescription()
+    ld.add_entity(declare_arguments())
+
+    ld.add_action(
+        Node(
+            package="moveit_ros_move_group",
+            executable="move_group",
+            output="screen",
+            parameters=[
+                moveit_config.to_dict(),
+                warehouse_ros_config,
+            ],
+        )
+    )
+
     rviz_config_file = PathJoinSubstitution(
         [FindPackageShare("ur_moveit_config"), "config", "moveit.rviz"]
     )
-    rviz_node = Node(
-        package="rviz2",
-        condition=IfCondition(launch_rviz),
-        executable="rviz2",
-        name="rviz2_moveit",
-        output="log",
-        arguments=["-d", rviz_config_file],
-        parameters=[
-            moveit_config.planning_pipelines,
-            moveit_config.robot_description_kinematics,
-        ],
+    ld.add_action(
+        Node(
+            package="rviz2",
+            condition=IfCondition(launch_rviz),
+            executable="rviz2",
+            name="rviz2_moveit",
+            output="log",
+            arguments=["-d", rviz_config_file],
+            parameters=[
+                moveit_config.robot_description,
+                moveit_config.robot_description_semantic,
+                moveit_config.robot_description_kinematics,
+                moveit_config.planning_pipelines,
+                moveit_config.joint_limits,
+                warehouse_ros_config,
+            ],
+        )
     )
 
-    ld = LaunchDescription()
-    ld.add_entity(declare_arguments())
-    ld.add_entity(generate_move_group_launch(moveit_config))
-    ld.add_action(rviz_node)
-
     return ld