Add move_to_pose_server and iiwa_msgs package with service definition

- Implement move_to_pose_server for motion planning using MoveIt
- Create iiwa_msgs package with MoveToPose service definition
- Update iiwa.launch.py to include move_to_pose_server node
- Modify joint_limits.yaml to add position limits for joints
- Remove iiwa_voice package and related files

src/iiwa_planning/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(iiwa_msgs REQUIRED)
 
 add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
 target_include_directories(motion_planning_cpp PUBLIC include)
@@ -37,11 +38,17 @@ install(TARGETS
 )
 
 install(PROGRAMS
-  # scripts/motion_planning_test.py
-  scripts/motion_planning.py
+  scripts/motion_planning_test.py
+  # scripts/motion_planning.py
   DESTINATION lib/${PROJECT_NAME}
   RENAME motion_planning
 )
 
+install(PROGRAMS
+  scripts/move_to_pose_server.py
+  DESTINATION lib/${PROJECT_NAME}
+  RENAME move_to_pose_server
+)
+
 
 ament_package()

src/iiwa_planning/package.xml

@@ -19,6 +19,7 @@
   <depend>geometry_msgs</depend>
   <depend>moveit_py</depend>
   <depend>tf_transformations</depend>
+  <depend>iiwa_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/iiwa_planning/scripts/motion_planning_test.py

@@ -78,7 +78,8 @@ def main():
     pose_goal.pose.position.x = 0.28
     pose_goal.pose.position.y = -0.2
     pose_goal.pose.position.z = 0.5
-    iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="link7")
+    iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, 
+                            pose_link="patron")
 
     # plan to goal
     plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from rclpy.callback_groups import ReentrantCallbackGroup
+from rclpy.executors import MultiThreadedExecutor, ExternalShutdownException
+from moveit.planning import MoveItPy, PlanningComponent
+from iiwa_msgs.srv import MoveToPose
+
+
+POSE_LINK = "link_ee"
+PLANNING_GROUP = "iiwa_arm"
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    # MoveItPy создаёт C++ узел с именем из лонча → читает robot_description_kinematics и т.д.
+    moveit = MoveItPy(node_name="move_to_pose_server")
+    arm: PlanningComponent = moveit.get_planning_component(PLANNING_GROUP)
+
+    # Отдельный лёгкий узел для сервиса — другое имя, нет конфликта параметров
+    node = Node("move_to_pose_service")
+    logger = node.get_logger()
+
+    cb_group = ReentrantCallbackGroup()
+
+    def handle(request: MoveToPose.Request, response: MoveToPose.Response):
+        pose = request.pose
+        if not pose.header.frame_id:
+            pose.header.frame_id = "base_link"
+
+        arm.set_start_state_to_current_state()
+        arm.set_goal_state(pose_stamped_msg=pose, pose_link=POSE_LINK)
+
+        logger.info(
+            f"Planning to ({pose.pose.position.x:.3f}, "
+            f"{pose.pose.position.y:.3f}, {pose.pose.position.z:.3f})"
+        )
+        plan_result = arm.plan()
+
+        if not plan_result:
+            response.success = False
+            response.message = "Planning failed: pose may be unreachable or in collision"
+            logger.error(response.message)
+            return response
+
+        moveit.execute(plan_result.trajectory, controllers=[])
+        response.success = True
+        response.message = "Motion executed successfully"
+        logger.info(response.message)
+        return response
+
+    node.create_service(MoveToPose, "iiwa/move_to_pose", handle, callback_group=cb_group)
+    logger.info(f"MoveToPoseServer ready (pose_link={POSE_LINK})")
+
+    try:
+        executor = MultiThreadedExecutor()
+        executor.add_node(node)
+        executor.spin()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()