【ROS】controller_manager卡在loading_controller

weixin_43326110

已于 2024-06-12 15:53:12 修改

阅读量1k

点赞数 5

CC 4.0 BY-SA版权

文章标签： ROS

于 2024-06-11 17:33:42 首次发布

本文链接：https://blog.csdn.net/weixin_43326110/article/details/139603881

项目场景：

最近为自己的机械臂基于hardware_interface写了一个驱动，并希望接入ros_control使用moveit进行控制，但是在controller_manager加载控制器的时候停在了loading_controller上。

问题描述

在launch中导入joint_state_controller 控制器

    <node name="rf_hybrid_arm_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller rf_hybrid_arm_controller" respawn="false" output="screen"/>

在驱动中进行了一个这样的循环

    ros::init(argc, argv, "rf_hybrid_arm_driver_node");
    
    RFHybridArmHW RFARM;    
    controller_manager::ControllerManager cm(&RFARM);
    ROS_INFO("Arm bring up successfully");

    // 控制循环
    ros::Rate loop_rate(RFARM.control_rate); // 控制频率为50Hz
    ros::Duration d(0.05);
    while (ros::ok())
    {
        RFARM.read();
        cm.update(ros::Time::now(), d);
        RFARM.write();
        loop_rate.sleep();
    }

    return 0;
}

这个基本就是参照ros_control的官方示例来的

hardware_interface · ros-controls/ros_control Wiki (github.com)

但是运行的时候总是会卡在loading_controller上，不会继续运行显示started_controller等,最开始我还以为别的哪里出问题了，一行一行排查了半天。

解决方案：

最后发现是在驱动的main函数里少了spin😂。

是的如果你的驱动里没有spin，那这个函数没法进行下去，因为controller_manager不知道你这表有没有加载成功控制器，所以他一直卡着。

我的驱动里确实也没有别的需要spin的地方，所以一开始就没写，不过ros官方教程里也没写，这个感觉有点被误导了，找了半天最后ros answer上找了一个老哥和我遇到的问题差不多，然后这里用了它的解决方法，在main里加一个spinner，完整的main函数如下：

How to load a controller from control_manager? - ROS Answers: Open Source Q&A Forum

int main(int argc, char** argv)
{
    ros::init(argc, argv, "rf_hybrid_arm_driver_node");
    
    RFHybridArmHW RFARM;    
    controller_manager::ControllerManager cm(&RFARM);
    ROS_INFO("Arm bring up successfully");

    // NOTE: We run the ROS loop in a separate thread as external calls such
    // as service callbacks to load controllers can block the (main) control loop
    ros::AsyncSpinner spinner(3);
    spinner.start();

    // 控制循环
    ros::Rate loop_rate(RFARM.control_rate); // 控制频率为50Hz
    ros::Duration d(0.05);
    while (ros::ok())
    {
        RFARM.read();
        cm.update(ros::Time::now(), d);
        RFARM.write();
        loop_rate.sleep();
    }

    return 0;
}