[运行 servo_example.launch.py 时遇到错误]:Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.

1656677130

背景：

我想使用 MoveIt Servo 来控制 servo_example.launch.py 中的示例机器人，使其到达一系列路径点。同时，我需要实时读取机器人的位置，以便可以在每个路径点前暂停或进行一些处理，然后继续移动到下一个路径点。

问题描述：

当我启动 servo_example.launch.py 并运行我的节点以读取机器人状态时，遇到了以下错误：

[current_endeffector_pose_publisher-5] [ERROR] [1732470180.400202271] [current_endeffector_pose_publisher]: Could not find parameter robot_description_semantic and did not receive robot_description_semantic via std_msgs::msg::String subscription within 10.000000 seconds.
[current_endeffector_pose_publisher-5] Error:   Could not parse the SRDF XML File. Error=XML_ERROR_EMPTY_DOCUMENT ErrorID=13 (0xd) Line number=0
[current_endeffector_pose_publisher-5]          at line 732 in /home/lsc/ws_moveit/src/srdfdom/src/model.cpp
[current_endeffector_pose_publisher-5] [ERROR] [1732470180.406341033] [moveit_rdf_loader.rdf_loader]: Unable to parse SRDF
[current_endeffector_pose_publisher-5] [FATAL] [1732470180.406906166] [move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
[current_endeffector_pose_publisher-5] terminate called without an active exception

具体细节和上下文：

我检查了互联网上相关问题的解决方案，比如在 servo_example.launch.py 中的 moveit_config 添加以下代码：

    .planning_scene_monitor(
        publish_robot_description=True, publish_robot_description_semantic=True
    )

但该方法对我的问题没有效果。

我进一步发现可能是某些话题缺失导致的，因此我检查了当前的 ROS 话题列表。发现没有 /robot_description_semantic 话题，只有 /robot_description。

但是，当我运行其他启动文件（例如 demo.launch.py）时，话题列表中会同时出现 /robot_description 和 /robot_description_semantic 当我仅运行 servo_example.launch.py 并使用键盘控制时，系统可以正常工作，没有报错。这表明问题可能不是来自 servo_example.launch.py 本身的基础配置，而是与我额外添加的节点（用于读取机器人状态）或其交互方式有关。

目前我不清楚如何生成 /robot_description_semantic 话题。

尝试过的解决方法：

按照网上的建议尝试在 servo_example.launch.py 中添加相关配置项，但未解决问题。
检查话题列表后发现缺少 /robot_description_semantic，并尝试对比其他启动文件的行为，但尚未找到问题的根源。

软件环境：

操作系统：Ubuntu 22.04
ROS 版本：Humble

1656677130

进一步信息

以下是我尝试读取机器人位姿的代码：

#include "robot_move_test/current_endeffector_pose_publisher.h"

CurrentEndeffectorPosePublisher::CurrentEndeffectorPosePublisher(const rclcpp::NodeOptions &options)
    : Node("CurrentEndeffectorPosePublisher", options) {
    RCLCPP_INFO(this->get_logger(), "CurrentEndeffectorPosePublisher node started.");


    // Create a publisher for the end-effector pose
    pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("endeffector_pose", 10);

    // Create a timer to periodically publish the pose
    timer_ = this->create_wall_timer(
        std::chrono::milliseconds(1200), // Publish every 1.2 seconds
        std::bind(&CurrentEndeffectorPosePublisher::publishPose, this));
}

void CurrentEndeffectorPosePublisher::initializeMoveGroup() {
    RCLCPP_INFO(this->get_logger(), "Initializing MoveGroupInterface...");
    try {
        // Initialize MoveGroupInterface
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "panda_arm");
        RCLCPP_INFO(this->get_logger(), "MoveGroupInterface initialized.");
    } catch (const std::exception &e) {
        RCLCPP_ERROR(this->get_logger(), "Exception during MoveGroupInterface initialization: %s", e.what());
    } catch (...) {
        RCLCPP_ERROR(this->get_logger(), "Unknown exception during MoveGroupInterface initialization.");
    }
}

void CurrentEndeffectorPosePublisher::publishPose() {
    if (!move_group_) {
        RCLCPP_WARN(this->get_logger(), "MoveGroupInterface not initialized yet!");
        return;
    }

    try {
        auto current_pose = move_group_->getCurrentPose();
        pose_publisher_->publish(current_pose);

        RCLCPP_INFO(this->get_logger(), "Current Endeffector Pose: pos_x=%.3f, pos_y=%.3f, pos_z=%.3f, quat_x=%.3f, quat_y=%.3f, quat_z=%.3f, quat_w=%.3f",
                    current_pose.pose.position.x,
                    current_pose.pose.position.y,
                    current_pose.pose.position.z,
                    current_pose.pose.orientation.x,
                    current_pose.pose.orientation.y,
                    current_pose.pose.orientation.z,
                    current_pose.pose.orientation.w);
    } catch (const std::exception &e) {
        RCLCPP_ERROR(this->get_logger(), "Exception during publishPose: %s", e.what());
    } catch (...) {
        RCLCPP_ERROR(this->get_logger(), "Unknown exception during publishPose.");
    }
}

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    try {
        auto node = std::make_shared<CurrentEndeffectorPosePublisher>(rclcpp::NodeOptions());
        node->initializeMoveGroup(); // Initialize MoveGroupInterface after the node is fully constructed
        rclcpp::spin(node);
    } catch (const std::exception &e) {
        std::cerr << "Exception in main: " << e.what() << std::endl;
    } catch (...) {
        std::cerr << "Unknown exception in main." << std::endl;
    }

    rclcpp::shutdown();
    return 0;
}