Multi-Robot Setup

rbs_skill_servers/CMakeLists.txt

@@ -33,7 +33,6 @@ find_package(tf2_msgs REQUIRED)
 find_package(tinyxml2_vendor REQUIRED)
 find_package(TinyXML2 REQUIRED)
 find_package(Eigen3 3.3 REQUIRED)
-find_package(nlohmann_json 3.2.0 REQUIRED)
 find_package(rbs_utils REQUIRED)
 
 # Default to Fortress
@@ -116,7 +115,7 @@ target_include_directories(
           $<INSTALL_INTERFACE:include>)
 target_compile_definitions(pick_place_pose_loader
                            PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
-ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3 nlohmann_json
+ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3 
                           rbs_utils)
 rclcpp_components_register_node(
   pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
@@ -134,30 +133,6 @@ add_executable(move_cartesian_path_action_server
                src/move_cartesian_path_action_server.cpp)
 ament_target_dependencies(move_cartesian_path_action_server ${deps})
 
-# add_executable(add_planning_scene_object_service
-#                src/add_planning_scene_objects_service.cpp)
-# ament_target_dependencies(add_planning_scene_object_service ${deps})
-
-# add_library(assemble_state_server SHARED src/assemble_state_server.cpp)
-# target_compile_definitions(assemble_state_server
-#                            PRIVATE "ASSEMBLE_STATE_SERVER_CPP_BUILDING_DLL")
-# ament_target_dependencies(assemble_state_server ${deps})
-# target_link_libraries(assemble_state_server ${TINYXML2_LIBRARY})
-# rclcpp_components_register_node(
-#   assemble_state_server PLUGIN "AssembleStateServer" EXECUTABLE
-#   assemble_state_service_server)
-
-# add_library(moveit_update_planning_scene_server SHARED
-#             src/moveit_update_planning_scene.cpp)
-# target_compile_definitions(
-#   moveit_update_planning_scene_server
-#   PRIVATE "MOVEIT_UPDATE_PLANNING_SCENE_SERVER_CPP_BUILDING_DLL")
-# ament_target_dependencies(moveit_update_planning_scene_server ${deps})
-# target_link_libraries(moveit_update_planning_scene_server ${TINYXML2_LIBRARY})
-# rclcpp_components_register_node(
-#   moveit_update_planning_scene_server PLUGIN "UpdatePlanningSceneServer"
-#   EXECUTABLE moveit_update_planning_scene_service_server)
-#
 install(DIRECTORY include/ DESTINATION include)
 
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
@@ -168,13 +143,16 @@ install(
           pick_place_pose_loader
           move_to_joint_states_action_server
           move_cartesian_path_action_server
-          # add_planning_scene_object_service
-          # assemble_state_server
-          # moveit_update_planning_scene_server
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 ament_export_include_directories(include)
 
+ament_python_install_package(${PROJECT_NAME})
+install(PROGRAMS
+  scripts/test_cartesian_controller.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 ament_package()

rbs_skill_servers/include/rbs_skill_servers/pick_place_pose_loader.hpp

@@ -3,8 +3,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_components/register_node_macro.hpp"
 #include <Eigen/Core>
-#include <nlohmann/json.hpp>
-#include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_ros/transform_listener.h>
 
@@ -24,8 +22,8 @@ private:
   geometry_msgs::msg::TransformStamped place_pose_tf;
   geometry_msgs::msg::TransformStamped grasp_pose_tf;
 
-  std::vector<geometry_msgs::msg::Pose>
-  getPlacePoseJson(const nlohmann::json &json);
+  // std::vector<geometry_msgs::msg::Pose>
+  // getPlacePoseJson(const nlohmann::json &json);
   void handleServer(
       const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
           request,
@@ -36,4 +34,4 @@ private:
 };
 } // namespace rbs_skill_actions
 
-RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);

rbs_skill_servers/launch/skills.launch.py

@@ -1,19 +1,97 @@
-import os
-from launch import LaunchDescription, LaunchContext
+from collections import namedtuple
+from os import name
+from launch import LaunchDescription
 from launch.actions import (
     DeclareLaunchArgument,
-    IncludeLaunchDescription,
-    ExecuteProcess,
     OpaqueFunction
 )
 from ament_index_python.packages import get_package_share_directory
-from launch.conditions import IfCondition, UnlessCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 from rbs_launch_utils.launch_common import load_yaml
 
+def launch_setup(context, *args, **kwargs):
+    robot_description_decl = LaunchConfiguration("robot_description")
+    robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
+    robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
+    use_sim_time = LaunchConfiguration("use_sim_time")
+    with_gripper_condition = LaunchConfiguration("with_gripper_condition")
+    points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
+    robot_description = {"robot_description": robot_description_decl}
+    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
+    namespace = LaunchConfiguration("namespace")
+
+    points_params = load_yaml(
+        "rbs_skill_servers", "config/gripperPositions.yaml"
+    )
+
+    move_topose_action_server = Node(
+        package="rbs_skill_servers",
+        executable="move_topose_action_server",
+        namespace=namespace,
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            {"use_sim_time": use_sim_time},
+        ]
+    )
+
+    gripper_control_node = Node(
+        package="rbs_skill_servers",
+        executable="gripper_control_action_server",
+        namespace=namespace,
+        parameters= [
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            {"use_sim_time": use_sim_time},
+        ],
+        condition=IfCondition(with_gripper_condition)
+    )
+
+    move_cartesian_path_action_server = Node(
+        package="rbs_skill_servers",
+        executable="move_cartesian_path_action_server",
+        namespace=namespace,
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            {"use_sim_time": use_sim_time},
+        ]
+    )
+    
+    # FIXME: The name of this node, "move_topose," 
+    # is intended to be different from the actual MoveToPose node.
+    move_joint_state_action_server = Node(
+        package="rbs_skill_servers",
+        executable="move_to_joint_states_action_server",
+        namespace=namespace,
+        parameters=[
+            robot_description,
+            robot_description_semantic,
+            robot_description_kinematics,
+            {"use_sim_time": use_sim_time},
+        ]
+    )
+        
+    grasp_pose_loader = Node(
+        package="rbs_skill_servers",
+        executable="pick_place_pose_loader_service_server",
+        namespace=namespace,
+        output="screen"
+    )
+
+    nodes_to_start =[
+        move_topose_action_server,
+        gripper_control_node,
+        move_cartesian_path_action_server,
+        move_joint_state_action_server,
+        # grasp_pose_loader
+    ]
+    return nodes_to_start
 
 def generate_launch_description():
     
@@ -41,77 +119,9 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument("points_params_filepath", default_value="")
     )
-
-    robot_description_decl = LaunchConfiguration("robot_description")
-    robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
-    robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
-    use_sim_time = LaunchConfiguration("use_sim_time")
-    with_gripper_condition = LaunchConfiguration("with_gripper_condition")
-    points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
-    robot_description = {"robot_description": robot_description_decl}
-    robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
-
-    points_params = load_yaml(
-        "rbs_skill_servers", "config/gripperPositions.yaml"
+    declared_arguments.append(
+        DeclareLaunchArgument("namespace", default_value="")
     )
 
-    move_topose_action_server = Node(
-        package="rbs_skill_servers",
-        executable="move_topose_action_server",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ]
-    )
 
-    gripper_control_node = Node(
-        package="rbs_skill_servers",
-        executable="gripper_control_action_server",
-        parameters= [
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ],
-        condition=IfCondition(with_gripper_condition)
-    )
-
-    move_cartesian_path_action_server = Node(
-        package="rbs_skill_servers",
-        executable="move_cartesian_path_action_server",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ]
-    )
-    
-    move_joint_state_action_server = Node(
-        package="rbs_skill_servers",
-        executable="move_to_joint_states_action_server",
-        parameters=[
-            robot_description,
-            robot_description_semantic,
-            robot_description_kinematics,
-            {"use_sim_time": use_sim_time},
-        ]
-    )
-        
-    grasp_pose_loader = Node(
-        package="rbs_skill_servers",
-        executable="pick_place_pose_loader_service_server",
-        output="screen"
-    )
-
-    nodes_to_start =[
-        move_topose_action_server,
-        gripper_control_node,
-        move_cartesian_path_action_server,
-        move_joint_state_action_server,
-        grasp_pose_loader
-    ]
-
-    return LaunchDescription(declared_arguments + nodes_to_start)
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

rbs_skill_servers/scripts/test_cartesian_controller.py

@@ -0,0 +1,47 @@
+import rclpy
+from rclpy.node import Node
+import argparse
+
+from geometry_msgs.msg import PoseStamped
+
+class CartesianControllerPublisher(Node):
+
+    def __init__(self, robot_name: str):
+        super().__init__("cartesian_controller_pose_publisher")
+        self.publisher_ = self.create_publisher(
+            PoseStamped, 
+            "/" + robot_name + "/cartesian_motion_controller/target_frame", 10)
+        timer_period = 0.5  # seconds
+        self.timer = self.create_timer(timer_period, self.timer_callback)
+
+    def timer_callback(self):
+        msg = PoseStamped()
+        msg.header.stamp = self.get_clock().now().to_msg()
+        msg.header.frame_id = "base_link"
+        msg.pose.position.x = 0.7
+        msg.pose.position.y = 0.0
+        msg.pose.position.z = 0.45
+        msg.pose.orientation.x = 0.0
+        msg.pose.orientation.y = 0.707
+        msg.pose.orientation.z = 0.0
+        msg.pose.orientation.w = 0.707
+
+        self.publisher_.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
+    parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
+    args = parser.parse_args()
+    minimal_publisher = CartesianControllerPublisher(args.robot_name)
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

rbs_skill_servers/scripts/test_move_to_pose.py

@@ -0,0 +1,69 @@
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.node import Node
+
+from rbs_skill_interfaces.action import MoveitSendPose
+
+
+class TestMoveToPose(Node):
+
+    def __init__(self, robot_name: str):
+        super().__init__('test_move_to_pose')
+        self._action_client = ActionClient(self, MoveitSendPose, "/"+ robot_name + "/move_topose")
+
+    def send_goal(self, msg):
+        goal_msg = MoveitSendPose.Goal()
+        # goal_msg.target_pose
+        goal_msg.target_pose.position.x = msg["target_pose"]["position"]["x"]
+        goal_msg.target_pose.position.y = msg["target_pose"]["position"]["y"]
+        goal_msg.target_pose.position.z = msg["target_pose"]["position"]["z"]
+
+        goal_msg.target_pose.orientation.x = msg["target_pose"]["orientation"]["x"]
+        goal_msg.target_pose.orientation.y = msg["target_pose"]["orientation"]["y"]
+        goal_msg.target_pose.orientation.z = msg["target_pose"]["orientation"]["z"]
+        goal_msg.target_pose.orientation.w = msg["target_pose"]["orientation"]["w"]
+
+        goal_msg.robot_name = msg["robot_name"]
+        goal_msg.end_effector_velocity = msg["end_effector_velocity"]
+        goal_msg.end_effector_acceleration = msg["end_effector_acceleration"]
+
+        self._action_client.wait_for_server()
+
+        return self._action_client.send_goal_async(goal_msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    robot_name = "arm0"
+
+    action_client = TestMoveToPose(robot_name)
+
+    msg = {
+        "constraint_mode": 0,
+        "target_pose": {
+            "position": {
+                "x": 0.2,
+                "y": 0.2,
+                "z": 0.35,
+            },
+            "orientation": {
+                "x": 0.0,
+                "y": 0.0,
+                "z": 0.0,
+                "w": 1.0,
+            },
+        },
+        "robot_name": robot_name,
+        "end_effector_velocity": 1.0,
+        "end_effector_acceleration": 1.0,
+        "timeout_seconds": 0.0
+    }
+
+    future = action_client.send_goal(msg)
+
+    rclpy.spin_until_future_complete(action_client, future)
+
+
+if __name__ == '__main__':
+    main()

rbs_skill_servers/src/gripper_control_action_server.cpp

@@ -31,7 +31,7 @@ public:
         std::bind(&GripperControlActionServer::accepted_callback, this,
                   std::placeholders::_1));
     publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(
-        "/gripper_controller/commands", 10);
+        "~/gripper_controller/commands", 10);
   }
 
 private:
@@ -96,10 +96,3 @@ private:
 } // namespace rbs_skill_actions
 
 RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
-
-// int main(int argc, char ** argv) {
-//     rclcpp::init(argc, argv);
-//     rbs_skill_actions::GripperControlActionServer server;
-//     rclcpp::spin(server);
-//     return 0;
-// }

rbs_skill_servers/src/move_to_joint_states_action_server.cpp

@@ -148,7 +148,8 @@ int main(int argc, char **argv) {
   rclcpp::init(argc, argv);
   rclcpp::NodeOptions node_options;
   node_options.automatically_declare_parameters_from_overrides(true);
-  auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
+  auto node =
+      rclcpp::Node::make_shared("move_to_joint_states", "", node_options);
 
   rbs_skill_actions::MoveToJointStateActionServer server(node);
   std::thread run_server([&server]() {

rbs_skill_servers/src/pick_place_pose_loader.cpp

@@ -7,7 +7,6 @@
 #include <fstream>
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 #include <memory>
-#include <nlohmann/json_fwd.hpp>
 #include <rclcpp/logging.hpp>
 #include <sdf/Geometry.hh>
 #include <tf2_eigen/tf2_eigen.hpp>
@@ -119,4 +118,4 @@ std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collectPose(
   poses.push_back(tf2::toMsg(postGraspPose));
 
   return poses;
-}
+}