Add standalone executable for Servo server node

moveit_ros/moveit_servo/CMakeLists.txt

@@ -20,6 +20,7 @@ set(SERVO_COMPONENT_NODE servo_server)
 set(SERVO_CONTROLLER_INPUT servo_controller_input)
 
 # Executable Nodes ##############################################
+set(SERVO_SERVER_NODE_NAME servo_server_node)
 set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
 set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
 
@@ -109,6 +110,11 @@ rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "moveit_servo::JoyToS
 ## Executable Nodes ##
 ######################
 
+# An executable node for the servo server
+add_executable(${SERVO_SERVER_NODE_NAME} src/servo_server_node.cpp)
+target_link_libraries(${SERVO_SERVER_NODE_NAME} ${SERVO_COMPONENT_NODE})
+ament_target_dependencies(${SERVO_SERVER_NODE_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 # An example of pose tracking
 add_executable(${POSE_TRACKING_DEMO_NAME} src/cpp_interface_demo/pose_tracking_demo.cpp)
 target_link_libraries(${POSE_TRACKING_DEMO_NAME} ${POSE_TRACKING})
@@ -144,6 +150,7 @@ install(
 # Install Binaries
 install(
   TARGETS
+    ${SERVO_SERVER_NODE_NAME}
     ${CPP_DEMO_NAME}
     ${POSE_TRACKING_DEMO_NAME}
     ${FAKE_SERVO_CMDS_NAME}

moveit_ros/moveit_servo/launch/servo_example.launch.py

@@ -0,0 +1,171 @@
+import os
+import yaml
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch.actions import ExecuteProcess
+import xacro
+
+
+def load_file(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return file.read()
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def load_yaml(package_name, file_path):
+    package_path = get_package_share_directory(package_name)
+    absolute_file_path = os.path.join(package_path, file_path)
+
+    try:
+        with open(absolute_file_path, "r") as file:
+            return yaml.safe_load(file)
+    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
+        return None
+
+
+def generate_launch_description():
+    # Get parameters for the Servo node
+    servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml")
+    servo_params = {"moveit_servo": servo_yaml}
+
+    # Get URDF and SRDF
+    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.toxml()}
+
+    robot_description_semantic_config = load_file(
+        "moveit_resources_panda_moveit_config", "config/panda.srdf"
+    )
+    robot_description_semantic = {
+        "robot_description_semantic": robot_description_semantic_config
+    }
+
+    # RViz
+    rviz_config_file = (
+        get_package_share_directory("moveit2_tutorials")
+        + "/config/demo_rviz_config.rviz"
+    )
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+        parameters=[robot_description, robot_description_semantic],
+    )
+
+    # 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 controllers
+    load_controllers = []
+    for controller in ["panda_arm_controller", "joint_state_broadcaster"]:
+        load_controllers += [
+            ExecuteProcess(
+                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
+                shell=True,
+                output="screen",
+            )
+        ]
+
+    # Launch as much as possible in components
+    container = ComposableNodeContainer(
+        name="moveit_servo_demo_container",
+        namespace="/",
+        package="rclcpp_components",
+        executable="component_container",
+        composable_node_descriptions=[
+            # Example of launching Servo as a node component
+            # ComposableNode(
+            #     package="moveit_servo",
+            #     plugin="moveit_servo::ServoServer",
+            #     name="servo_server",
+            #     parameters=[
+            #         servo_params,
+            #         robot_description,
+            #         robot_description_semantic,
+            #     ],
+            #     extra_arguments=[{"use_intra_process_comms": True}],
+            # ),
+            ComposableNode(
+                package="robot_state_publisher",
+                plugin="robot_state_publisher::RobotStatePublisher",
+                name="robot_state_publisher",
+                parameters=[robot_description],
+            ),
+            ComposableNode(
+                package="tf2_ros",
+                plugin="tf2_ros::StaticTransformBroadcasterNode",
+                name="static_tf2_broadcaster",
+                parameters=[{"child_frame_id": "/panda_link0", "frame_id": "/world"}],
+            ),
+            ComposableNode(
+                package="moveit_servo",
+                plugin="moveit_servo::ServoServer",
+                name="servo_server",
+                parameters=[
+                    servo_params,
+                    robot_description,
+                    robot_description_semantic,
+                ],
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package="moveit_servo",
+                plugin="moveit_servo::JoyToServoPub",
+                name="controller_to_servo_node",
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+            ComposableNode(
+                package="joy",
+                plugin="joy::Joy",
+                name="joy_node",
+                extra_arguments=[{"use_intra_process_comms": True}],
+            ),
+        ],
+        output="screen",
+    )
+
+    servo_node = Node(
+        package="moveit_servo",
+        executable="servo_server_node",
+        parameters=[
+            servo_params,
+            robot_description,
+            robot_description_semantic,
+        ],
+        output={
+            "stdout": "screen",
+            "stderr": "screen",
+        },
+    )
+
+    return LaunchDescription(
+        [rviz_node, ros2_control_node, servo_node, container] + load_controllers
+    )

moveit_ros/moveit_servo/src/servo_server_node.cpp

@@ -0,0 +1,54 @@
+/*******************************************************************************
+ * BSD 3-Clause License
+ *
+ * Copyright (c) 2021, PickNik Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice, this
+ *   list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright notice,
+ *   this list of conditions and the following disclaimer in the documentation
+ *   and/or other materials provided with the distribution.
+ *
+ * * Neither the name of the copyright holder nor the names of its
+ *   contributors may be used to endorse or promote products derived from
+ *   this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *******************************************************************************/
+
+/*      Title     : servo_server_node.cpp
+ *      Project   : moveit_servo
+ *      Created   : 08/18/2021
+ *      Author    : Joe Schornak
+ */
+
+#include <moveit_servo/servo_server.h>
+
+int main(int argc, char* argv[])
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::NodeOptions options;
+  options.automatically_declare_parameters_from_overrides(true);
+
+  auto node = std::make_shared<moveit_servo::ServoServer>(options);
+
+  rclcpp::spin(node);
+
+  rclcpp::shutdown();
+}