rbs_gym multi camera and multi object setup

rbs_skill_servers/CMakeLists.txt

@@ -34,7 +34,7 @@ find_package(tinyxml2_vendor REQUIRED)
 find_package(TinyXML2 REQUIRED)
 find_package(Eigen3 3.3 REQUIRED)
 find_package(rbs_utils REQUIRED)
-find_package(moveit_servo REQUIRED)
+# find_package(moveit_servo REQUIRED)
 
 # Default to Fortress
 set(SDF_VER 12)
@@ -80,7 +80,7 @@ set(deps
     moveit_ros_planning
     moveit_ros_planning_interface
     moveit_msgs
-    moveit_servo
+    # moveit_servo
     geometry_msgs
     tf2_ros
     rclcpp_components

rbs_skill_servers/launch/skills.launch.py

@@ -88,15 +88,17 @@ def launch_setup(context, *args, **kwargs):
             {"use_sim_time": use_sim_time},
         ]
     )
-        
-    grasp_pose_loader = Node(
-        package="rbs_skill_servers",
-        executable="pick_place_pose_loader_service_server",
+
+    assembly_config = Node(
+        package="rbs_utils",
+        executable="assembly_config_service.py",
         namespace=namespace,
         output="screen"
     )
+        
 
     nodes_to_start =[
+        assembly_config,
         move_topose_action_server,
         gripper_control_node,
         move_cartesian_path_action_server,

rbs_skill_servers/package.xml

@@ -13,7 +13,7 @@
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend>
   <depend>moveit_ros_planning_interface</depend>
-  <depend>moveit_servo</depend>
+  <!-- <depend>moveit_servo</depend> -->
   <depend>moveit_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>rclcpp_action</depend>

rbs_skill_servers/scripts/move_to_pose.py

@@ -48,11 +48,13 @@ class CartesianMoveToPose(Node):
         self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}")
         target_pose = goal_handle.request.target_pose
         start_pose = self.current_pose.pose if self.current_pose else None
+        result = MoveitSendPose.Result()
 
         if start_pose is None:
             self.get_logger().error("Current pose is not available")
             goal_handle.abort()
-            return MoveitSendPose.Result()
+            result.success = False
+            return result
 
         trajectory = self.generate_trajectory(start_pose, target_pose)
         for point in trajectory:
@@ -64,8 +66,6 @@ class CartesianMoveToPose(Node):
             rclpy.spin_once(self, timeout_sec=0.1)
 
         goal_handle.succeed()
-
-        result = MoveitSendPose.Result()
         result.success = True
         return result
 

rbs_skill_servers/scripts/test_cartesian_controller.py

@@ -39,7 +39,7 @@ class CartesianControllerPublisher(Node):
 def main(args=None):
     rclpy.init(args=args)
 
-    parser = argparse.ArgumentParser(description='ROS2 Minimal Publisher')
+    parser = argparse.ArgumentParser()
     parser.add_argument('--robot-name', type=str, default='arm0', help='Specify the robot name')
     args = parser.parse_args()