项目场景:

最近为自己的机械臂基于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;
}

 参考资料

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

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

ros_control - ROS Wiki

点赞(0) 打赏

评论列表 共有 0 条评论

暂无评论

微信公众账号

微信扫一扫加关注

发表
评论
返回
顶部