# 主要功能
1. 订阅RGB+Depth图像和相机信息
2. 调用YOLO+SAM进行目标检测和分割
3. 将2D检测结果转换为3D语义点云
4. 发布语义对象消息

# 主要功能
1. 接收语义对象消息
2. 管理语义地图数据库
3. 处理对象跟踪和更新
4. 发布可视化标记

# 主要功能
1. 提供语义查询服务
2. 结合LLM/VLM进行智能问答
3. 生成导航指导

# 主要功能
1. 订阅RGB+Depth图像和相机信息
2. 调用YOLO+SAM进行目标检测和分割
3. 将2D检测结果转换为3D语义点云
4. 发布语义对象消息

# 主要功能
1. 接收语义对象消息
2. 管理语义地图数据库
3. 处理对象跟踪和更新
4. 发布可视化标记

# 主要功能
1. 提供语义查询服务
2. 结合LLM/VLM进行智能问答
3. 生成导航指导

# scripts/llm.py
from openai import OpenAI
import os

class LLM:
    def __init__(self, model="deepseek-ai/DeepSeek-V2.5", api_key=None):
        self.api_key = api_key or os.environ.get("SILICONFLOW_API_KEY")
        self.client = OpenAI(
            base_url="https://api.siliconflow.cn/v1/",
            api_key=self.api_key
        )
        self.model = model

    def chat(self, messages, **kwargs):
        response = self.client.chat.completions.create(
            model=self.model,
            messages=messages,
            **kwargs
        )
        return response.choices[0].message.content

# scripts/llm_node.py
import rospy
from llm_pkg.srv import LLMChat, LLMChatResponse
from llm import LLM
import json

class LLMNode:
    def __init__(self):
        rospy.init_node("llm_node")
        self.llm = LLM()

        # 加载配置
        with open("scripts/llm_types.json", "r") as f:
            self.llm_types = json.load(f)

        # 创建服务
        self.service = rospy.Service("llm_chat", LLMChat, self.handle_chat)

    def handle_chat(self, req):
        try:
            messages = [
                {"role": "system", "content": self.llm_types["chat"][req.type]["system_prompt"]},
                {"role": "user", "content": req.content}
            ]
            response = self.llm.chat(messages)
            return LLMChatResponse(success=True, response=response)
        except Exception as e:
            return LLMChatResponse(success=False, response=str(e))

# scripts/semantic_map_generator_node.py
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from message_filters import ApproximateTimeSynchronizer, Subscriber
from cv_bridge import CvBridge

class SemanticMapGenerator:
    def __init__(self):
        rospy.init_node("semantic_map_generator")

        # 订阅器
        image_sub = Subscriber("/camera/color/image_raw", Image)
        depth_sub = Subscriber("/camera/aligned_depth_to_color/image_raw", Image)
        camera_info_sub = Subscriber("/camera/color/camera_info", CameraInfo)

        # 时间同步
        self.ts = ApproximateTimeSynchronizer(
            [image_sub, depth_sub, camera_info_sub],
            queue_size=5,
            slop=0.05
        )
        self.ts.registerCallback(self.sync_callback)

        # 发布器
        self.semantic_pub = rospy.Publisher("/semantic_object", SemanticObject, queue_size=10)

        # 检测服务客户端
        rospy.wait_for_service("/vit_detection")
        self.detector = rospy.ServiceProxy("/vit_detection", VitDetection)

        self.bridge = CvBridge()

    def sync_callback(self, img_msg, depth_msg, camera_info_msg):
        # 处理同步的图像数据
        rgb = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
        depth = self.bridge.imgmsg_to_cv2(depth_msg, "passthrough")

        # 调用检测服务
        results = self.detector(rgb, self.current_prompt)

        # 处理检测结果并发布语义对象
        self.process_detection_results(results, rgb, depth, camera_info_msg)

# scripts/lib/semantic_map_database.py
import sqlite3
import numpy as np
import cv2
import os

class SemanticMapDatabase:
    def __init__(self, db_path, img_dir, renew_db=False):
        self.db_path = db_path
        self.img_dir = img_dir

        if renew_db and os.path.exists(db_path):
            os.remove(db_path)

        self._init_db()

    def _init_db(self):
        conn = sqlite3.connect(self.db_path)
        conn.execute("""
            CREATE TABLE IF NOT EXISTS semantic_objects (
                label TEXT PRIMARY KEY,
                bbox BLOB,
                time_stamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP,
                x_data BLOB,
                y_data BLOB,
                z_data BLOB,
                rgb_data BLOB,
                feature_data BLOB
            )
        """)
        conn.commit()
        conn.close()

    def update_entry(self, label, bbox, x, y, z, rgb, feature):
        conn = sqlite3.connect(self.db_path)
        conn.execute("""
            INSERT OR REPLACE INTO semantic_objects
            (label, bbox, x_data, y_data, z_data, rgb_data, feature_data)
            VALUES (?, ?, ?, ?, ?, ?, ?)
        """, (label, bbox, x, y, z, rgb, feature))
        conn.commit()
        conn.close()

# scripts/llm.py
from openai import OpenAI
import os

class LLM:
    def __init__(self, model="deepseek-ai/DeepSeek-V2.5", api_key=None):
        self.api_key = api_key or os.environ.get("SILICONFLOW_API_KEY")
        self.client = OpenAI(
            base_url="https://api.siliconflow.cn/v1/",
            api_key=self.api_key
        )
        self.model = model

    def chat(self, messages, **kwargs):
        response = self.client.chat.completions.create(
            model=self.model,
            messages=messages,
            **kwargs
        )
        return response.choices[0].message.content

# scripts/llm_node.py
import rospy
from llm_pkg.srv import LLMChat, LLMChatResponse
from llm import LLM
import json

class LLMNode:
    def __init__(self):
        rospy.init_node("llm_node")
        self.llm = LLM()

        # 加载配置
        with open("scripts/llm_types.json", "r") as f:
            self.llm_types = json.load(f)

        # 创建服务
        self.service = rospy.Service("llm_chat", LLMChat, self.handle_chat)

    def handle_chat(self, req):
        try:
            messages = [
                {"role": "system", "content": self.llm_types["chat"][req.type]["system_prompt"]},
                {"role": "user", "content": req.content}
            ]
            response = self.llm.chat(messages)
            return LLMChatResponse(success=True, response=response)
        except Exception as e:
            return LLMChatResponse(success=False, response=str(e))

# scripts/semantic_map_generator_node.py
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from message_filters import ApproximateTimeSynchronizer, Subscriber
from cv_bridge import CvBridge

class SemanticMapGenerator:
    def __init__(self):
        rospy.init_node("semantic_map_generator")

        # 订阅器
        image_sub = Subscriber("/camera/color/image_raw", Image)
        depth_sub = Subscriber("/camera/aligned_depth_to_color/image_raw", Image)
        camera_info_sub = Subscriber("/camera/color/camera_info", CameraInfo)

        # 时间同步
        self.ts = ApproximateTimeSynchronizer(
            [image_sub, depth_sub, camera_info_sub],
            queue_size=5,
            slop=0.05
        )
        self.ts.registerCallback(self.sync_callback)

        # 发布器
        self.semantic_pub = rospy.Publisher("/semantic_object", SemanticObject, queue_size=10)

        # 检测服务客户端
        rospy.wait_for_service("/vit_detection")
        self.detector = rospy.ServiceProxy("/vit_detection", VitDetection)

        self.bridge = CvBridge()

    def sync_callback(self, img_msg, depth_msg, camera_info_msg):
        # 处理同步的图像数据
        rgb = self.bridge.imgmsg_to_cv2(img_msg, "bgr8")
        depth = self.bridge.imgmsg_to_cv2(depth_msg, "passthrough")

        # 调用检测服务
        results = self.detector(rgb, self.current_prompt)

        # 处理检测结果并发布语义对象
        self.process_detection_results(results, rgb, depth, camera_info_msg)

# scripts/lib/semantic_map_database.py
import sqlite3
import numpy as np
import cv2
import os

class SemanticMapDatabase:
    def __init__(self, db_path, img_dir, renew_db=False):
        self.db_path = db_path
        self.img_dir = img_dir

        if renew_db and os.path.exists(db_path):
            os.remove(db_path)

        self._init_db()

    def _init_db(self):
        conn = sqlite3.connect(self.db_path)
        conn.execute("""
            CREATE TABLE IF NOT EXISTS semantic_objects (
                label TEXT PRIMARY KEY,
                bbox BLOB,
                time_stamp TIMESTAMP DEFAULT CURRENT_TIMESTAMP,
                x_data BLOB,
                y_data BLOB,
                z_data BLOB,
                rgb_data BLOB,
                feature_data BLOB
            )
        """)
        conn.commit()
        conn.close()

    def update_entry(self, label, bbox, x, y, z, rgb, feature):
        conn = sqlite3.connect(self.db_path)
        conn.execute("""
            INSERT OR REPLACE INTO semantic_objects
            (label, bbox, x_data, y_data, z_data, rgb_data, feature_data)
            VALUES (?, ?, ?, ?, ?, ?, ?)
        """, (label, bbox, x, y, z, rgb, feature))
        conn.commit()
        conn.close()

# interact环境中的依赖
torch==2.5.0
torchvision==0.20.0
funasr==1.2.6
chattts==0.0.0

# py310环境中的依赖
torch==2.6.0
torchvision==0.21.0
ultralytics==8.3.94
segment-anything==1.0

# interact环境中的依赖
torch==2.5.0
torchvision==0.20.0
funasr==1.2.6
chattts==0.0.0

# py310环境中的依赖
torch==2.6.0
torchvision==0.21.0
ultralytics==8.3.94
segment-anything==1.0

# Cerebellum模块调用Brain的LLM服务
class SemanticMapGuide:
    def __init__(self):
        # 等待LLM服务就绪
        rospy.wait_for_service("llm_chat")
        rospy.wait_for_service("llm_reason")

        # 创建服务客户端
        self.llm_chat_client = rospy.ServiceProxy("llm_chat", LLMChat)
        self.llm_reason_client = rospy.ServiceProxy("llm_reason", LLMChat)

    def ask_llm_chat(self, type, content):
        """调用LLM对话服务"""
        try:
            response = self.llm_chat_client(type, content)
            return response
        except rospy.ServiceException as e:
            rospy.logerr(f"LLM chat service call failed: {e}")
            return None

    def ask_llm_reason(self, type, content):
        """调用LLM推理服务"""
        try:
            response = self.llm_reason_client(type, content)
            return response
        except rospy.ServiceException as e:
            rospy.logerr(f"LLM reason service call failed: {e}")
            return None

# Cerebellum模块调用Brain的VLM服务
class SemanticMapGuide:
    def __init__(self):
        # 等待VLM服务就绪
        rospy.wait_for_service("vlm_chat")
        self.vlm_chat_client = rospy.ServiceProxy("vlm_chat", VLMChat)

    def ask_vlm_chat(self, type, content, image_path):
        """调用VLM视觉问答服务"""
        try:
            response = self.vlm_chat_client(type, content, image_path)
            return response
        except rospy.ServiceException as e:
            rospy.logerr(f"VLM chat service call failed: {e}")
            return None

# Cerebellum模块调用Brain的CLIP服务
class SemanticMapGuide:
    def __init__(self):
        # 等待CLIP服务就绪
        rospy.wait_for_service("clip")
        self.clip_client = rospy.ServiceProxy("clip", Clip)

    def lang_imgs_match(self, lang, img_paths):
        """调用CLIP进行语言-图像匹配"""
        response = self.clip_client(
            task="text_match_images",
            text=lang,
            image_paths=img_paths
        )
        return response

# Cerebellum发布语义对象数据
class SemanticMapGenerator:
    def __init__(self):
        # 发布语义对象
        self.semantic_object_pub = rospy.Publisher(
            "/semantic_object", SemanticObject, queue_size=10
        )

    def publish_semantic_object(self, semantic_obj):
        """发布语义对象信息"""
        self.semantic_object_pub.publish(semantic_obj)

# Cerebellum发布机器人位姿
class SemanticMapGuide:
    def __init__(self):
        # 订阅机器人位姿
        rospy.Subscriber(
            "/robot_pose_ekf/odom_combined",
            PoseWithCovarianceStamped,
            self.pose_callback
        )

    def pose_callback(self, msg):
        """机器人位姿回调"""
        self.robot_pose = msg.pose.pose

# 1. 用户指令输入
user_command = "把桌子上的饮料放到冰箱里"

# 2. Cerebellum调用Brain进行任务规划
def execute_tasks(self, cmd):
    # 构建任务规划请求
    content = f"以下是语义对象列表：{self.semantic_categories}。用户的指令是：{cmd}"

    # 调用Brain的LLM推理服务
    llm_reason_res = self.ask_llm_reason("task_plan_reason", content)

    # 解析任务计划
    task_plans = self.llm_analyzer.analyze("task_plan_reason", llm_reason_res.response)

    # 执行任务计划
    for task_plan in task_plans:
        self.execute_task_plan(task_plan, cmd)

# 1. 语义对象查找
def find_semantic_object_by_language(self, category, language):
    # 获取图像路径
    label_img_paths = self.database.get_img_paths_by_category(category)

    # 调用Brain的CLIP服务进行匹配
    match_pairs = self.lang_imgs_match(language, img_paths)

    # 返回最佳匹配结果
    return best_label

# 1. 接收导航请求
def guide_callback(self, req):
    # 2. 调用Brain进行语义理解
    content = f"要找的对象是{req.category}，用户的指令是{req.origin_cmd}"
    llm_chat_res = self.ask_llm_chat("category_or_language_chat", content)

    # 3. 解析语义对象属性
    navi_obj_properties = self.llm_analyzer.analyze(
        "category_or_language_chat", llm_chat_res.response
    )

    # 4. 查找具体语义对象
    if navi_obj_properties["type"] == "language":
        label = self.find_semantic_object_by_language(
            navi_obj_properties["object"],
            navi_obj_properties["description"]
        )

    # 5. 生成导航目标
    ideal_navi_goals = self.get_ideal_navi_goals(label, req.op_cmd)

    # 6. 返回导航指导结果
    return GuideResponse(
        success=True,
        label=label,
        nav_goals=ideal_navi_goals
    )

# 等待服务就绪
rospy.wait_for_service("llm_chat")
rospy.wait_for_service("vlm_chat")
rospy.wait_for_service("clip")

try:
    response = self.llm_chat_client(type, content)
    return response
except rospy.ServiceException as e:
    rospy.logerr(f"Service call failed: {e}")
    return None

# 设置服务调用超时
rospy.wait_for_service("llm_chat", timeout=10.0)

# Cerebellum模块调用Brain的LLM服务
class SemanticMapGuide:
    def __init__(self):
        # 等待LLM服务就绪
        rospy.wait_for_service("llm_chat")
        rospy.wait_for_service("llm_reason")

        # 创建服务客户端
        self.llm_chat_client = rospy.ServiceProxy("llm_chat", LLMChat)
        self.llm_reason_client = rospy.ServiceProxy("llm_reason", LLMChat)

    def ask_llm_chat(self, type, content):
        """调用LLM对话服务"""
        try:
            response = self.llm_chat_client(type, content)
            return response
        except rospy.ServiceException as e:
            rospy.logerr(f"LLM chat service call failed: {e}")
            return None

    def ask_llm_reason(self, type, content):
        """调用LLM推理服务"""
        try:
            response = self.llm_reason_client(type, content)
            return response
        except rospy.ServiceException as e:
            rospy.logerr(f"LLM reason service call failed: {e}")
            return None

# Cerebellum模块调用Brain的VLM服务
class SemanticMapGuide:
    def __init__(self):
        # 等待VLM服务就绪
        rospy.wait_for_service("vlm_chat")
        self.vlm_chat_client = rospy.ServiceProxy("vlm_chat", VLMChat)

    def ask_vlm_chat(self, type, content, image_path):
        """调用VLM视觉问答服务"""
        try:
            response = self.vlm_chat_client(type, content, image_path)
            return response
        except rospy.ServiceException as e:
            rospy.logerr(f"VLM chat service call failed: {e}")
            return None

# Cerebellum模块调用Brain的CLIP服务
class SemanticMapGuide:
    def __init__(self):
        # 等待CLIP服务就绪
        rospy.wait_for_service("clip")
        self.clip_client = rospy.ServiceProxy("clip", Clip)

    def lang_imgs_match(self, lang, img_paths):
        """调用CLIP进行语言-图像匹配"""
        response = self.clip_client(
            task="text_match_images",
            text=lang,
            image_paths=img_paths
        )
        return response

# Cerebellum发布语义对象数据
class SemanticMapGenerator:
    def __init__(self):
        # 发布语义对象
        self.semantic_object_pub = rospy.Publisher(
            "/semantic_object", SemanticObject, queue_size=10
        )

    def publish_semantic_object(self, semantic_obj):
        """发布语义对象信息"""
        self.semantic_object_pub.publish(semantic_obj)

# Cerebellum发布机器人位姿
class SemanticMapGuide:
    def __init__(self):
        # 订阅机器人位姿
        rospy.Subscriber(
            "/robot_pose_ekf/odom_combined",
            PoseWithCovarianceStamped,
            self.pose_callback
        )

    def pose_callback(self, msg):
        """机器人位姿回调"""
        self.robot_pose = msg.pose.pose

# 1. 用户指令输入
user_command = "把桌子上的饮料放到冰箱里"

# 2. Cerebellum调用Brain进行任务规划
def execute_tasks(self, cmd):
    # 构建任务规划请求
    content = f"以下是语义对象列表：{self.semantic_categories}。用户的指令是：{cmd}"

    # 调用Brain的LLM推理服务
    llm_reason_res = self.ask_llm_reason("task_plan_reason", content)

    # 解析任务计划
    task_plans = self.llm_analyzer.analyze("task_plan_reason", llm_reason_res.response)

    # 执行任务计划
    for task_plan in task_plans:
        self.execute_task_plan(task_plan, cmd)

# 1. 语义对象查找
def find_semantic_object_by_language(self, category, language):
    # 获取图像路径
    label_img_paths = self.database.get_img_paths_by_category(category)

    # 调用Brain的CLIP服务进行匹配
    match_pairs = self.lang_imgs_match(language, img_paths)

    # 返回最佳匹配结果
    return best_label

# 1. 接收导航请求
def guide_callback(self, req):
    # 2. 调用Brain进行语义理解
    content = f"要找的对象是{req.category}，用户的指令是{req.origin_cmd}"
    llm_chat_res = self.ask_llm_chat("category_or_language_chat", content)

    # 3. 解析语义对象属性
    navi_obj_properties = self.llm_analyzer.analyze(
        "category_or_language_chat", llm_chat_res.response
    )

    # 4. 查找具体语义对象
    if navi_obj_properties["type"] == "language":
        label = self.find_semantic_object_by_language(
            navi_obj_properties["object"],
            navi_obj_properties["description"]
        )

    # 5. 生成导航目标
    ideal_navi_goals = self.get_ideal_navi_goals(label, req.op_cmd)

    # 6. 返回导航指导结果
    return GuideResponse(
        success=True,
        label=label,
        nav_goals=ideal_navi_goals
    )

# 等待服务就绪
rospy.wait_for_service("llm_chat")
rospy.wait_for_service("vlm_chat")
rospy.wait_for_service("clip")

try:
    response = self.llm_chat_client(type, content)
    return response
except rospy.ServiceException as e:
    rospy.logerr(f"Service call failed: {e}")
    return None

# 设置服务调用超时
rospy.wait_for_service("llm_chat", timeout=10.0)

# 发布者
self.pub = rospy.Publisher('/semantic_object', SemanticObject, queue_size=10)
self.pub.publish(semantic_obj)

# 订阅者
rospy.Subscriber('/semantic_object', SemanticObject, self.callback)

# 服务客户端
rospy.wait_for_service('llm_chat')
client = rospy.ServiceProxy('llm_chat', LLMChat)
response = client(type, content)

# 服务服务器
rospy.Service('llm_chat', LLMChat, self.handle_request)

# 发布者
self.pub = rospy.Publisher('/semantic_object', SemanticObject, queue_size=10)
self.pub.publish(semantic_obj)

# 订阅者
rospy.Subscriber('/semantic_object', SemanticObject, self.callback)

# 服务客户端
rospy.wait_for_service('llm_chat')
client = rospy.ServiceProxy('llm_chat', LLMChat)
response = client(type, content)

# 服务服务器
rospy.Service('llm_chat', LLMChat, self.handle_request)

class LLM:
    def __init__(
        self,
        model: str = "deepseek-ai/DeepSeek-V2.5",
        max_tokens: int = 4096,
        base_url: str = "https://api.siliconflow.cn/v1/",
        api_key: Optional[str] = None,
    ):
        self.model = model
        self.max_tokens = max_tokens
        self.base_url = base_url
        self.api_key = api_key or os.environ.get("SILICONFLOW_API_KEY")

        if not self.api_key:
            raise ValueError("未找到API密钥，请设置环境变量 SILICONFLOW_API_KEY")

        self.client = OpenAI(base_url=self.base_url, api_key=self.api_key)

def _process_response(self, response: Any, stream: bool) -> Generator[str, None, None]:
    if stream:
        # 流式响应处理
        first_content = True
        first_reason = True
        for chunk in response:
            delta = chunk.choices[0].delta

            if delta.content:
                new_content = delta.content
                if first_content:
                    yield "\n<ans>\n"
                    first_content = False
                yield from new_content

            if hasattr(delta, "reasoning_content") and delta.reasoning_content:
                new_reason = delta.reasoning_content
                if first_reason:
                    yield "\n<think>\n"
                    first_reason = False
                yield from new_reason
    else:
        # 非流式响应处理
        completion = response.choices[0].message
        if completion.content:
            yield f"\n<ans>\n{completion.content}"
        if hasattr(completion, "reasoning_content") and completion.reasoning_content:
            yield f"\n<think>\n{completion.reasoning_content}"

def stream_chat(self, messages: list, **kwargs) -> Generator[str, None, None]:
    """流式对话"""
    response = self.client.chat.completions.create(
        model=self.model,
        messages=messages,
        stream=True,
        max_tokens=self.max_tokens,
        **kwargs,
    )
    return self._process_response(response, stream=True)

def non_stream_chat(self, messages: list, **kwargs) -> str:
    """非流式对话"""
    response = self.client.chat.completions.create(
        model=self.model,
        messages=messages,
        stream=False,
        max_tokens=self.max_tokens,
        **kwargs,
    )
    return "".join(self._process_response(response, stream=False))

class LLMNode:
    def __init__(self):
        rospy.init_node("llm_node")

        # 初始化两个LLM实例
        self.chat_llm = LLM("deepseek-ai/DeepSeek-V2.5")  # 对话模型
        self.reason_llm = LLM("deepseek-ai/DeepSeek-R1")  # 推理模型

        # 加载配置
        llm_types_path = rospy.get_param("~llm_types_path", default_llm_types_path)
        self.llm_types = json.load(open(llm_types_path, "r"))
        self.chat_types = self.llm_types["chat"].keys()
        self.reason_types = self.llm_types["reason"].keys()

        # 创建ROS服务
        self.service = rospy.Service("llm_chat", LLMChat, self.handle_llm_chat_request)
        self.service_reason = rospy.Service("llm_reason", LLMChat, self.handle_llm_reason_request)

def _process_response(self, response, type="chat"):
    if type == "chat":
        # 对话响应处理
        res = response.strip()
        res = res.replace("<ans>", "").replace("<think>", "")
        res = res.replace("\n", "")
    elif type == "reason":
        # 推理响应处理
        res = response.strip()
        res = res.replace("\n", "")
        # 提取<ans>和<think>标签之间的内容
        pattern = r"<ans>(.*?)<think>"
        matches = re.findall(pattern, res)
        if matches:
            res = matches[0].strip()
        else:
            rospy.logerr("Did not find <ans> and <think> tags in the response.")
            res = None
    return res

def handle_llm_chat_request(self, req):
    """处理对话请求"""
    try:
        type = req.type
        if type not in self.chat_types:
            rospy.logwarn(f"不支持的问答类型: {type}")
            type = "test_chat"

        # 构建消息
        messages = [
            {
                "role": "system",
                "content": self.llm_types["chat"][type]["system_prompt"],
            },
            {"role": "user", "content": req.content},
        ]

        # 调用LLM
        response = self.chat_llm.non_stream_chat(messages)
        response = self._process_response(response, "chat")
        return LLMChatResponse(success=True, response=str(response))

    except Exception as e:
        rospy.logerr(f"处理LLM Chat请求时发生错误: {str(e)}")
        return LLMChatResponse(success=False, response=str(e))

class LLMAnalyzer:
    def analyze(self, type, response):
        """分析LLM响应"""
        if type == "category_or_language_chat":
            return self._analyze_category_or_language(response)
        elif type == "task_plan_reason":
            return self._analyze_task_plan(response)
        else:
            return f"failed_to_parse, unsupported type: {type}"

def _analyze_category_or_language(self, response):
    """分析类别或语言描述"""
    # 找出{}之间的内容
    category_or_language = re.findall(r"\{(.*?)\}", response)
    if category_or_language:
        try:
            category_or_language = ast.literal_eval(
                "{" + category_or_language[0] + "}"
            )
            return category_or_language  # dict
        except Exception as e:
            return f"Failed to parse: {str(e)}\noriginal: {response}"
    else:
        return "failed_to_parse, no match"

def _analyze_task_plan(self, response):
    """分析任务规划"""
    # 检查是否包含"not_in_list"
    if "not_in_list" in response.lower():
        return "not_in_list"

    # 找出[]之间的内容
    task_plan_match = re.findall(r"\[(.*?)\]", response, re.DOTALL)
    if task_plan_match:
        try:
            # 使用ast.literal_eval解析Python字面量
            task_plans = ast.literal_eval(f"[{task_plan_match[0]}]")
            return task_plans  # list
        except Exception as e:
            # 尝试JSON解析
            try:
                modified_response = task_plan_match[0].replace("'", '"')
                task_plans = json.loads(f"[{modified_response}]")
                return task_plans
            except json.JSONDecodeError:
                return "failed_to_parse, decode error"
    else:
        return "failed_to_parse, no match"

import rospy
from llm_pkg.srv import LLMChat
from llm_analyzer.llm_analyzer import LLMAnalyzer

# 等待服务
rospy.wait_for_service("llm_chat")
rospy.wait_for_service("llm_reason")

# 创建客户端
llm_chat_client = rospy.ServiceProxy("llm_chat", LLMChat)
llm_reason_client = rospy.ServiceProxy("llm_reason", LLMChat)

# 创建分析器
llm_analyzer = LLMAnalyzer()

# 调用对话服务
response = llm_chat_client("test_chat", "你好")
print(f"对话响应: {response.response}")

# 调用推理服务
response = llm_reason_client("task_plan_reason", "用户指令内容")
task_plans = llm_analyzer.analyze("task_plan_reason", response.response)
print(f"任务计划: {task_plans}")

def analyze(self, type, response):
    if type == "new_type":
        return self._analyze_new_type(response)
    # ... 其他类型处理

class LLM:
    def __init__(
        self,
        model: str = "deepseek-ai/DeepSeek-V2.5",
        max_tokens: int = 4096,
        base_url: str = "https://api.siliconflow.cn/v1/",
        api_key: Optional[str] = None,
    ):
        self.model = model
        self.max_tokens = max_tokens
        self.base_url = base_url
        self.api_key = api_key or os.environ.get("SILICONFLOW_API_KEY")

        if not self.api_key:
            raise ValueError("未找到API密钥，请设置环境变量 SILICONFLOW_API_KEY")

        self.client = OpenAI(base_url=self.base_url, api_key=self.api_key)

def _process_response(self, response: Any, stream: bool) -> Generator[str, None, None]:
    if stream:
        # 流式响应处理
        first_content = True
        first_reason = True
        for chunk in response:
            delta = chunk.choices[0].delta

            if delta.content:
                new_content = delta.content
                if first_content:
                    yield "\n<ans>\n"
                    first_content = False
                yield from new_content

            if hasattr(delta, "reasoning_content") and delta.reasoning_content:
                new_reason = delta.reasoning_content
                if first_reason:
                    yield "\n<think>\n"
                    first_reason = False
                yield from new_reason
    else:
        # 非流式响应处理
        completion = response.choices[0].message
        if completion.content:
            yield f"\n<ans>\n{completion.content}"
        if hasattr(completion, "reasoning_content") and completion.reasoning_content:
            yield f"\n<think>\n{completion.reasoning_content}"

def stream_chat(self, messages: list, **kwargs) -> Generator[str, None, None]:
    """流式对话"""
    response = self.client.chat.completions.create(
        model=self.model,
        messages=messages,
        stream=True,
        max_tokens=self.max_tokens,
        **kwargs,
    )
    return self._process_response(response, stream=True)

def non_stream_chat(self, messages: list, **kwargs) -> str:
    """非流式对话"""
    response = self.client.chat.completions.create(
        model=self.model,
        messages=messages,
        stream=False,
        max_tokens=self.max_tokens,
        **kwargs,
    )
    return "".join(self._process_response(response, stream=False))

class LLMNode:
    def __init__(self):
        rospy.init_node("llm_node")

        # 初始化两个LLM实例
        self.chat_llm = LLM("deepseek-ai/DeepSeek-V2.5")  # 对话模型
        self.reason_llm = LLM("deepseek-ai/DeepSeek-R1")  # 推理模型

        # 加载配置
        llm_types_path = rospy.get_param("~llm_types_path", default_llm_types_path)
        self.llm_types = json.load(open(llm_types_path, "r"))
        self.chat_types = self.llm_types["chat"].keys()
        self.reason_types = self.llm_types["reason"].keys()

        # 创建ROS服务
        self.service = rospy.Service("llm_chat", LLMChat, self.handle_llm_chat_request)
        self.service_reason = rospy.Service("llm_reason", LLMChat, self.handle_llm_reason_request)