Use fake_components::GenericSystem from ros2_control (moveit#361)

* Use GenericSystem for moveit_servo, moveit_demo_nodes, planning_interface tests * Remove fake-joint from .repos + update ros2_control branches * Move moveit_test_utils outside BUILD_TESTING if statement * Remove mongodb_server_node from test launch file * Fix target dependencies for rviz_plugin_render_tools * Fix installation directory for run_moveit_cpp, run_ompl_constrained_planning
yangtfu · Mar 25, 2021 · 2620f7b · 2620f7b
1 parent c9c61a6
commit 2620f7b
Show file tree

Hide file tree

Showing 42 changed files with 586 additions and 496 deletions.
diff --git a/moveit2.repos b/moveit2.repos
@@ -19,28 +19,18 @@ repositories:
     type: git
     url: https://github.com/ros-planning/srdfdom
     version: ros2
-
-# TODO(#283): Switch to master after fixing breaking changes in ros2_control
   ros2_control:
     type: git
     url: https://github.com/ros-controls/ros2_control
-    version: 2c3aad1cb8ad4d23925bceeb5c26e22d6cf859ef
+    version: master
   ros2_controllers:
     type: git
     url: https://github.com/ros-controls/ros2_controllers
-    version: 8587e079490acdd0fabeb2742a3bec3bf551bbc5
-
-# TODO(JafarAbdi): Switch to upstream once PR https://github.com/tork-a/fake_joint/pull/7 is merged
-  fake_joint:
-    type: git
-    url: https://github.com/JafarAbdi/fake_joint
-    version: foxy
-
+    version: master
   warehouse_ros:
     type: git
     url: https://github.com/ros-planning/warehouse_ros
     version: ros2
-
   warehouse_ros_mongo:
     type: git
     url: https://github.com/ros-planning/warehouse_ros_mongo

diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt
@@ -10,23 +10,18 @@ set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_V
 
 install(DIRECTORY include/ DESTINATION include)
 
-if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
-  find_package(ament_index_cpp REQUIRED)
-  set(MOVEIT_TEST_LIB_NAME moveit_test_utils)
-  add_library(${MOVEIT_TEST_LIB_NAME} SHARED src/robot_model_test_utils.cpp)
-  target_link_libraries(${MOVEIT_TEST_LIB_NAME} moveit_robot_model)
-  ament_target_dependencies(${MOVEIT_TEST_LIB_NAME}
-    ament_index_cpp
-    Boost
-    geometry_msgs
-    urdf
-    srdfdom
-    urdfdom
-    urdfdom_headers
-  )
 
-  set_target_properties(${MOVEIT_TEST_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-
-  ament_export_libraries(${MOVEIT_TEST_LIB_NAME} ${MOVEIT_LIB_NAME})
-endif()
+find_package(ament_index_cpp REQUIRED)
+set(MOVEIT_TEST_LIB_NAME moveit_test_utils)
+add_library(${MOVEIT_TEST_LIB_NAME} SHARED src/robot_model_test_utils.cpp)
+target_link_libraries(${MOVEIT_TEST_LIB_NAME} moveit_robot_model)
+ament_target_dependencies(${MOVEIT_TEST_LIB_NAME}
+  ament_index_cpp
+  Boost
+  geometry_msgs
+  urdf
+  srdfdom
+  urdfdom
+  urdfdom_headers
+)
+set_target_properties(${MOVEIT_TEST_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
diff --git a/moveit_demo_nodes/run_move_group/CMakeLists.txt b/moveit_demo_nodes/run_move_group/CMakeLists.txt
@@ -37,9 +37,6 @@ ament_export_targets(export_${PROJECT_NAME})
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )
-install(DIRECTORY config
-  DESTINATION share/${PROJECT_NAME}
-)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

diff --git a/moveit_demo_nodes/run_move_group/config/controllers.yaml b/moveit_demo_nodes/run_move_group/config/controllers.yaml
diff --git a/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py b/moveit_demo_nodes/run_move_group/launch/run_move_group.launch.py
@@ -2,7 +2,9 @@
 import yaml
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
 from ament_index_python.packages import get_package_share_directory
+import xacro
 
 
 def load_file(package_name, file_path):
@@ -30,10 +32,14 @@ def load_yaml(package_name, file_path):
 def generate_launch_description():
 
     # planning_context
-    robot_description_config = load_file(
-        "moveit_resources_panda_description", "urdf/panda.urdf"
+    robot_description_config = xacro.process_file(
+        os.path.join(
+            get_package_share_directory("moveit_resources_panda_moveit_config"),
+            "config",
+            "panda.urdf.xacro",
+        )
     )
-    robot_description = {"robot_description": robot_description_config}
+    robot_description = {"robot_description": robot_description_config.toxml()}
 
     robot_description_semantic_config = load_file(
         "moveit_resources_panda_moveit_config", "config/panda.srdf"
@@ -61,9 +67,11 @@ def generate_launch_description():
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
     # Trajectory Execution Functionality
-    controllers_yaml = load_yaml("run_move_group", "config/controllers.yaml")
+    moveit_simple_controllers_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml"
+    )
     moveit_controllers = {
-        "moveit_simple_controller_manager": controllers_yaml,
+        "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
         "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
     }
 
@@ -133,25 +141,46 @@ def generate_launch_description():
         parameters=[robot_description],
     )
 
-    # Fake joint driver
-    fake_joint_driver_node = Node(
-        package="fake_joint_driver",
-        executable="fake_joint_driver_node",
-        parameters=[
-            {"controller_name": "panda_arm_controller"},
-            os.path.join(
-                get_package_share_directory("run_move_group"),
-                "config",
-                "panda_controllers.yaml",
-            ),
-            os.path.join(
-                get_package_share_directory("run_move_group"),
-                "config",
-                "start_positions.yaml",
-            ),
-            robot_description,
-        ],
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "panda_ros_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, ros2_controllers_path],
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+    )
+
+    # load joint_state_controller
+    load_joint_state_controller = ExecuteProcess(
+        cmd=["ros2 control load_start_controller joint_state_controller"],
+        shell=True,
+        output="screen",
     )
+    load_controllers = [load_joint_state_controller]
+    # load panda_arm_controller
+    load_controllers += [
+        ExecuteProcess(
+            cmd=["ros2 control load_configure_controller panda_arm_controller"],
+            shell=True,
+            output="screen",
+            on_exit=[
+                ExecuteProcess(
+                    cmd=[
+                        "ros2 control switch_controllers --start-controllers panda_arm_controller"
+                    ],
+                    shell=True,
+                    output="screen",
+                )
+            ],
+        )
+    ]
 
     # Warehouse mongodb server
     mongodb_server_node = Node(
@@ -171,7 +200,8 @@ def generate_launch_description():
             static_tf,
             robot_state_publisher,
             run_move_group_node,
-            fake_joint_driver_node,
+            ros2_control_node,
             mongodb_server_node,
         ]
+        + load_controllers
     )
diff --git a/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt b/moveit_demo_nodes/run_moveit_cpp/CMakeLists.txt
@@ -30,7 +30,7 @@ install(TARGETS run_moveit_cpp
   EXPORT export_${PROJECT_NAME}
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION bin
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
   INCLUDES DESTINATION include
 )
 ament_export_targets(export_${PROJECT_NAME})

diff --git a/moveit_demo_nodes/run_moveit_cpp/config/controllers.yaml b/moveit_demo_nodes/run_moveit_cpp/config/controllers.yaml
diff --git a/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py b/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp.launch.py
@@ -2,7 +2,9 @@
 import yaml
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch.actions import ExecuteProcess
 from ament_index_python.packages import get_package_share_directory
+import xacro
 
 
 def load_file(package_name, file_path):
@@ -34,10 +36,14 @@ def generate_launch_description():
     )
 
     # Component yaml files are grouped in separate namespaces
-    robot_description_config = load_file(
-        "moveit_resources_panda_description", "urdf/panda.urdf"
+    robot_description_config = xacro.process_file(
+        os.path.join(
+            get_package_share_directory("moveit_resources_panda_moveit_config"),
+            "config",
+            "panda.urdf.xacro",
+        )
     )
-    robot_description = {"robot_description": robot_description_config}
+    robot_description = {"robot_description": robot_description_config.toxml()}
 
     robot_description_semantic_config = load_file(
         "moveit_resources_panda_moveit_config", "config/panda.srdf"
@@ -51,9 +57,11 @@ def generate_launch_description():
     )
     robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
 
-    controllers_yaml = load_yaml("run_moveit_cpp", "config/controllers.yaml")
+    moveit_simple_controllers_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/panda_controllers.yaml"
+    )
     moveit_controllers = {
-        "moveit_simple_controller_manager": controllers_yaml,
+        "moveit_simple_controller_manager": moveit_simple_controllers_yaml,
         "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
     }
 
@@ -118,34 +126,54 @@ def generate_launch_description():
         parameters=[robot_description],
     )
 
-    # Fake joint driver
-    fake_joint_driver_node = Node(
-        package="fake_joint_driver",
-        executable="fake_joint_driver_node",
-        # TODO(JafarAbdi): Why this launch the two nodes (controller manager and the fake joint driver) with the same name!
-        # name='fake_joint_driver_node',
-        parameters=[
-            {"controller_name": "panda_arm_controller"},
-            os.path.join(
-                get_package_share_directory("run_moveit_cpp"),
-                "config",
-                "panda_controllers.yaml",
-            ),
-            os.path.join(
-                get_package_share_directory("run_moveit_cpp"),
-                "config",
-                "start_positions.yaml",
-            ),
-            robot_description,
-        ],
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "panda_ros_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, ros2_controllers_path],
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+    )
+
+    # load joint_state_controller
+    load_joint_state_controller = ExecuteProcess(
+        cmd=["ros2 control load_start_controller joint_state_controller"],
+        shell=True,
+        output="screen",
     )
+    load_controllers = [load_joint_state_controller]
+    # load panda_arm_controller
+    load_controllers += [
+        ExecuteProcess(
+            cmd=["ros2 control load_configure_controller panda_arm_controller"],
+            shell=True,
+            output="screen",
+            on_exit=[
+                ExecuteProcess(
+                    cmd=[
+                        "ros2 control switch_controllers --start-controllers panda_arm_controller"
+                    ],
+                    shell=True,
+                    output="screen",
+                )
+            ],
+        )
+    ]
 
     return LaunchDescription(
         [
             static_tf,
             robot_state_publisher,
             rviz_node,
             run_moveit_cpp_node,
-            fake_joint_driver_node,
+            ros2_control_node,
         ]
+        + load_controllers
     )
diff --git a/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp_fake_controller.launch.py b/moveit_demo_nodes/run_moveit_cpp/launch/run_moveit_cpp_fake_controller.launch.py
@@ -70,9 +70,11 @@ def generate_launch_description():
         "trajectory_execution.allowed_start_tolerance": 0.01,
     }
 
-    controllers_yaml = load_yaml("run_moveit_cpp", "config/fake_controllers.yaml")
+    moveit_simple_controllers_yaml = load_yaml(
+        "moveit_resources_panda_moveit_config", "config/fake_controllers.yaml"
+    )
     fake_controller = {
-        "moveit_fake_controller_manager": controllers_yaml,
+        "moveit_fake_controller_manager": moveit_simple_controllers_yaml,
         "moveit_controller_manager": "moveit_fake_controller_manager/MoveItFakeControllerManager",
     }
 

diff --git a/moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt b/moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt
@@ -26,7 +26,7 @@ install(TARGETS run_ompl_constrained_planning
   EXPORT export_${PROJECT_NAME}
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
-  RUNTIME DESTINATION bin
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
   INCLUDES DESTINATION include
 )
 ament_export_targets(export_${PROJECT_NAME})
@@ -35,8 +35,4 @@ install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME}
 )
 
-install(DIRECTORY config
-  DESTINATION share/${PROJECT_NAME}
-)
-
 ament_package()