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_bringup/launch/iiwa.launch.py

@@ -211,6 +211,17 @@ def _runtime_setup(context, *args, **kwargs):
         ],
     )
 
+    move_to_pose_server = Node(
+        package="iiwa_planning",
+        executable="move_to_pose_server",
+        output="screen",
+        parameters=[
+            moveit_configs.to_dict(),
+            {"robot_description": robot_description},
+            {"use_sim_time": use_sim_time},
+        ],
+    )
+
     # Rviz launch
     rviz_launch = Node(
         condition=IfCondition(LaunchConfiguration("rviz")),
@@ -237,10 +248,11 @@ def _runtime_setup(context, *args, **kwargs):
     )
 
     setup += [
-        controllers_launch, 
+        controllers_launch,
-        move_group, 
+        move_group,
-        rviz_launch, 
+        move_to_pose_server,
-        shutdown_on_rviz_exit
+        rviz_launch,
+        shutdown_on_rviz_exit,
     ]
 
     if settings.foxglove.enabled:

src/iiwa_config/config/moveit/iiwa7.srdf

@@ -10,7 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="iiwa_arm">
-        <joint name="world_base_joint"/>
+        <!-- <joint name="world_base_joint"/>
         <joint name="joint1"/>
         <joint name="joint2"/>
         <joint name="joint3"/>
@@ -23,7 +23,8 @@
         <joint name="camera_holder_patron"/>
         <joint name="camera_holder_corner"/>
         <joint name="camera_corner_camera"/>
-        <joint name="camera_hand_to_optical"/>
+        <joint name="camera_hand_to_optical"/> -->
+        <chain base_link="base_link" tip_link="patron"/>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="home" group="iiwa_arm">

src/iiwa_config/config/moveit/joint_limits.yaml

@@ -9,38 +9,66 @@ default_acceleration_scaling_factor: 0.1
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
   joint1:
+    has_position_limits: true
+    min_position: -2.97
+    max_position: 2.97
     has_velocity_limits: true
     max_velocity: 1.71
     has_acceleration_limits: true
     max_acceleration: 8.5521
+    has_jerk_limits: false
   joint2:
+    has_position_limits: true
+    min_position: -2.09
+    max_position: 2.09
     has_velocity_limits: true
     max_velocity: 1.71
     has_acceleration_limits: true
     max_acceleration: 8.5521
+    has_jerk_limits: false
   joint3:
+    has_position_limits: true
+    min_position: -2.97
+    max_position: 2.97
     has_velocity_limits: true
     max_velocity: 1.75
     has_acceleration_limits: true
     max_acceleration: 8.7266
+    has_jerk_limits: false
   joint4:
+    has_position_limits: true
+    min_position: -2.09
+    max_position: 2.09
     has_velocity_limits: true
     max_velocity: 2.27
     has_acceleration_limits: true
     max_acceleration: 11.3446
+    has_jerk_limits: false
   joint5:
+    has_position_limits: true
+    min_position: -2.97
+    max_position: 2.97
     has_velocity_limits: true
     max_velocity: 2.4399999999999999
     has_acceleration_limits: true
     max_acceleration: 12.2173
+    has_jerk_limits: false
   joint6:
+    has_position_limits: true
+    min_position: -2.09
+    max_position: 2.09
     has_velocity_limits: true
     max_velocity: 3.1400000000000001
     has_acceleration_limits: true
     max_acceleration: 15.7080
+    has_jerk_limits: false
   joint7:
+    has_position_limits: true
+    min_position: -3.05
+    max_position: 3.05
     has_velocity_limits: true
     max_velocity: 3.1400000000000001
     has_acceleration_limits: true
     max_acceleration: 15.7080
+    has_jerk_limits: false
     

src/iiwa_msgs/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(iiwa_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/MoveToPose.srv"
+  DEPENDENCIES geometry_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

src/iiwa_msgs/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>iiwa_msgs</name>
+  <version>0.0.0</version>
+  <description>Custom ROS2 interfaces for KUKA iiwa7</description>
+  <maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+  <depend>geometry_msgs</depend>
+  <depend>rosidl_default_runtime</depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/iiwa_msgs/srv/MoveToPose.srv

@@ -0,0 +1,4 @@
+geometry_msgs/PoseStamped pose
+---
+bool success
+string message

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_test.py
-  scripts/motion_planning.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()

src/iiwa_voice/iiwa_voice/voice_generate.py

@@ -1,95 +0,0 @@
-import httpx
-import sounddevice as sd
-import numpy as np
-import io
-import wave
-import argparse
-
-ENDPOINTS = {
-    "customvoice": "http://localhost:8091",
-    "voicedesign": "http://localhost:8092",
-    "base": "http://localhost:8093",
-}
-
-def synthesize_and_play(
-    text: str,
-    voice: str = "Vivian",
-    model_type: str = "customvoice",
-    language: str = "English",
-    instruct: str = "",
-    base_url: str | None = None,
-):
-    url = (base_url or ENDPOINTS[model_type]) + "/v1/audio/speech"
-
-    model_map = {
-        "customvoice": "Qwen/Qwen3-TTS-12Hz-0.6B-CustomVoice",
-        "voicedesign": "Qwen/Qwen3-TTS-12Hz-0.6B-VoiceDesign",
-        "base": "Qwen/Qwen3-TTS-12Hz-0.6B-Base",
-    }
-
-    payload = {
-        "model": model_map[model_type],
-        "input": text,
-        "voice": voice,
-        "language": language,
-        "response_format": "wav",
-    }
-    if instruct:
-        payload["instruct"] = instruct
-
-    print(f"[→] Отправка запроса к {url} ...")
-
-    with httpx.Client(timeout=120) as client:
-        response = client.post(url, json=payload)
-        response.raise_for_status()
-        audio_bytes = response.content
-
-    # Читаем WAV из памяти — без записи на диск
-    with wave.open(io.BytesIO(audio_bytes)) as wf:
-        sample_rate = wf.getframerate()
-        n_channels = wf.getnchannels()
-        sample_width = wf.getsampwidth()   # байт на семпл
-        frames = wf.readframes(wf.getnframes())
-
-    # Конвертируем байты → numpy array
-    dtype_map = {1: np.int8, 2: np.int16, 4: np.int32}
-    dtype = dtype_map.get(sample_width, np.int16)
-    audio_np = np.frombuffer(frames, dtype=dtype)
-
-    if n_channels > 1:
-        audio_np = audio_np.reshape(-1, n_channels)
-
-    # Нормализуем до float32 [-1.0, 1.0] для sounddevice
-    audio_float = audio_np.astype(np.float32) / np.iinfo(dtype).max
-
-    duration = len(audio_float) / sample_rate
-    print(f"[♪] Воспроизведение: {duration:.1f} сек | {sample_rate} Hz | {n_channels}ch")
-
-    sd.play(audio_float, samplerate=sample_rate)
-    sd.wait()   # блокируемся до конца воспроизведения
-    print("[✓] Готово")
-
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description="Qwen3-TTS клиент")
-    parser.add_argument("--text", default="Hello! This is Qwen3-TTS speaking.")
-    parser.add_argument("--voice", default="Vivian",
-                        help="Имя голоса (CustomVoice) или описание (VoiceDesign)")
-    parser.add_argument("--type", default="customvoice",
-                        choices=["customvoice", "voicedesign", "base"])
-    parser.add_argument("--language", default="English")
-    parser.add_argument("--instruct", default="",
-                        help="Дополнительная инструкция: 'speak slowly', 'angry tone' и т.п.")
-    parser.add_argument("--url", default=None,
-                        help="Переопределить базовый URL, напр. http://myserver:8091")
-    args = parser.parse_args()
-
-    synthesize_and_play(
-        text=args.text,
-        voice=args.voice,
-        model_type=args.type,
-        language=args.language,
-        instruct=args.instruct,
-        base_url=args.url,
-    )
-    

src/iiwa_voice/package.xml

@@ -1,18 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>iiwa_voice</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="grabardm@ml-dev.ru">daniel</maintainer>
-  <license>TODO: License declaration</license>
-
-  <test_depend>ament_copyright</test_depend>
-  <test_depend>ament_flake8</test_depend>
-  <test_depend>ament_pep257</test_depend>
-  <test_depend>python3-pytest</test_depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>

src/iiwa_voice/resource/.env

@@ -1,40 +0,0 @@
-# =============================================================================
-#  Qwen3-TTS — настройки окружения (.env)
-#  Скопируйте этот файл в .env и заполните своими значениями
-# =============================================================================
-
-# --- Hugging Face ---------------------------------------------------------
-# Токен нужен только если модели приватные или у вас rate-limit
-HF_TOKEN=
-
-# Путь для кэша весов моделей (рекомендуется SSD с ~20 ГБ свободного места)
-HF_CACHE_DIR=./hf_cache
-
-# --- GPU ------------------------------------------------------------------
-# Сколько GPU выделить каждому сервису (обычно 1)
-GPU_COUNT=1
-
-# Видимые GPU (например "0" или "0,1" для multi-GPU)
-CUDA_VISIBLE_DEVICES=0
-
-# Утилизация памяти GPU (0.0–1.0). Уменьшите при OOM.
-GPU_MEM_UTIL=0.90
-
-# Максимальная длина контекста
-MAX_MODEL_LEN=4096
-
-# --- Порты ----------------------------------------------------------------
-CUSTOMVOICE_PORT=8091
-VOICEDESIGN_PORT=8092
-BASE_PORT=8093
-
-# --- Модели ---------------------------------------------------------------
-# По умолчанию 1.7B. Замените на 0.6B для экономии VRAM (~4 ГБ вместо ~8 ГБ)
-# MODEL_SIZE=Qwen/Qwen3-TTS-12Hz-1.7B-CustomVoice
-MODEL_SIZE=Qwen/Qwen3-TTS-12Hz-0.6B-CustomVoice
-
-# MODEL_VOICEDESIGN=Qwen/Qwen3-TTS-12Hz-1.7B-VoiceDesign
-MODEL_VOICEDESIGN=Qwen/Qwen3-TTS-12Hz-0.6B-VoiceDesign
-
-# MODEL_BASE=Qwen/Qwen3-TTS-12Hz-1.7B-Base
-MODEL_BASE=Qwen/Qwen3-TTS-12Hz-0.6B-Base

src/iiwa_voice/resource/docker-compose.yaml

@@ -1,101 +0,0 @@
-# =============================================================================
-#  Qwen3-TTS — vLLM-Omni  (docker-compose.yml)
-#  Три варианта модели на разных портах:
-#    customvoice  → :8091  (заранее настроенные голоса: Vivian, Ryan …)
-#    voicedesign  → :8092  (создание голоса по текстовому описанию)
-#    base         → :8093  (клонирование голоса из ~3 сек. аудио)
-#
-#  Запуск всех сервисов:    docker compose --profile all up -d
-#  Только один сервис:      docker compose --profile customvoice up -d
-#  Логи:                    docker compose logs -f qwen3-tts-customvoice
-# =============================================================================
-
-x-tts-common: &tts-common
-  image: vllm/vllm-omni:v0.18.0
-  ipc: host
-  entrypoint: ["vllm", "serve"]
-  ulimits:
-    memlock: -1
-    stack: 67108864
-  restart: unless-stopped
-  deploy:
-    resources:
-      reservations:
-        devices:
-          - driver: nvidia
-            count: ${GPU_COUNT:-1}
-            capabilities: [gpu]
-  volumes:
-    - ${HF_CACHE_DIR:-./hf_cache}:/root/.cache/huggingface
-    - ./output_audio:/output_audio
-  environment:
-    HF_TOKEN: ${HF_TOKEN:-}
-    VLLM_WORKER_MULTIPROC_METHOD: spawn
-    CUDA_VISIBLE_DEVICES: ${CUDA_VISIBLE_DEVICES:-0}
-  healthcheck:
-    test: ["CMD", "python3", "-c",
-           "import urllib.request; urllib.request.urlopen('http://localhost:8091/health')"]
-    interval: 30s
-    timeout: 10s
-    retries: 5
-    start_period: 120s
-
-# =============================================================================
-services:
-
-  # CustomVoice — готовые голоса (Vivian, Ryan и др.)
-  qwen3-tts-customvoice:
-    <<: *tts-common
-    container_name: qwen3-tts-customvoice
-    ports:
-      - "${CUSTOMVOICE_PORT:-8091}:8091"
-    command:
-      - "${MODEL_CUSTOMVOICE:-Qwen/Qwen3-TTS-12Hz-0.63B-CustomVoice}"
-      - "--omni"
-      - "--port"
-      - "8091"
-      - "--gpu-memory-utilization"
-      - "${GPU_MEM_UTIL:-0.90}"
-      - "--max-model-len"
-      - "${MAX_MODEL_LEN:-4096}"
-    profiles:
-      - customvoice
-      - all
-
-  # VoiceDesign — синтез голоса по описанию на естественном языке
-  qwen3-tts-voicedesign:
-    <<: *tts-common
-    container_name: qwen3-tts-voicedesign
-    ports:
-      - "${VOICEDESIGN_PORT:-8092}:8091"
-    command:
-      - "${MODEL_VOICEDESIGN:-Qwen/Qwen3-TTS-12Hz-0.63B-VoiceDesign}"
-      - "--omni"
-      - "--port"
-      - "8091"
-      - "--gpu-memory-utilization"
-      - "${GPU_MEM_UTIL:-0.90}"
-      - "--max-model-len"
-      - "${MAX_MODEL_LEN:-4096}"
-    profiles:
-      - voicedesign
-      - all
-
-  # Base — клонирование голоса из референс-аудио
-  qwen3-tts-base:
-    <<: *tts-common
-    container_name: qwen3-tts-base
-    ports:
-      - "${BASE_PORT:-8093}:8091"
-    command:
-      - "${MODEL_BASE:-Qwen/Qwen3-TTS-12Hz-0.63B-Base}"
-      - "--omni"
-      - "--port"
-      - "8091"
-      - "--gpu-memory-utilization"
-      - "${GPU_MEM_UTIL:-0.90}"
-      - "--max-model-len"
-      - "${MAX_MODEL_LEN:-4096}"
-    profiles:
-      - base
-      - all

src/iiwa_voice/setup.cfg

@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/iiwa_voice
-[install]
-install_scripts=$base/lib/iiwa_voice

src/iiwa_voice/setup.py

@@ -1,29 +0,0 @@
-from setuptools import find_packages, setup
-
-package_name = 'iiwa_voice'
-
-setup(
-    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-    ],
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='daniel',
-    maintainer_email='grabardm@ml-dev.ru',
-    description='TODO: Package description',
-    license='TODO: License declaration',
-    extras_require={
-        'test': [
-            'pytest',
-        ],
-    },
-    entry_points={
-        'console_scripts': [
-        ],
-    },
-)