Webots_ros头文件，去除冗余代码，它来了

最新推荐文章于 2024-11-07 17:06:27 发布

原创

最新推荐文章于 2024-11-07 17:06:27 发布 · 2.3k 阅读

4 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #c++ #人工智能

本文档介绍了如何通过创建Webots_ROS头文件来整理和优化原始代码，减少冗余，提高工作效率。作者展示了如何将常见的控制代码封装到类中，包括初始化、电机控制等函数，并提供了简化后的程序示例，帮助读者理解这一优化过程。

Webots_ros整合头文件，去除冗余代码，它来了（Demo）

闲聊

时隔3个月，我再一次来写关于Webots和Ros的教程了。

在这3个月内一直在努力工作，早上9点多上班，下班没有准确的时间，6点，7点，8点。。。都是可能的。加上连续的出差，也使我力不从心，一回到酒店就立马洗洗睡了。

从停更的那几篇文章中也能看到，我更新文章的时间都在晚上9点-12点之间，这还是在我刚刚入职的时候=—=

在这里插入图片描述

写文章不易，希望多多支持^.

废话不多说，进入正题。

本来的机器人控制代码

这边使用webots_demo的velocity_keyboard_v2.cpp代码，大家都知道，这是控制webots电机的代码，一共226行，有多少是冗余代码呢(70行)…

可以看到所有和电机有关的源文件中都有如下程序

    while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
        ros::spinOnce();
    }
    ros::spinOnce();
    // 服务订阅time_step和webots保持同步
    timeStepClient = n->serviceClient<webots_ros::set_int>("robot/robot/time_step");
    timeStepSrv.request.value = TIME_STEP;

    // 如果在webots中有多个控制器的话，需要让用户选择一个控制器
    if (controllerCount == 1)
        controllerName = controllerList[0];
    else {
        int wantedController = 0;
        std::cout << "Choose the # of the controller you want to use:\n";
        std::cin >> wantedController;
        if (1 <= wantedController && wantedController <= controllerCount)
            controllerName = controllerList[wantedController - 1];
        else {
            ROS_ERROR("Invalid number for controller choice.");
        return 1;
        }
    }
    ROS_INFO("Using controller: '%s'", controllerName.c_str());
    // 退出主题，因为已经不重要了
    nameSub.shutdown();

    //初始化电机
    for (int i = 0; i < NMOTORS; ++i) {
        // position速度控制时设置为缺省值INFINITY   
        set_position_client = n->serviceClient<webots_ros::set_float>(string(ROBOT_NAME) + string(motorNames[i]) + string("/set_position"));   
        set_position_srv.request.value = INFINITY;
        if (set_position_client.call(set_position_srv) && set_position_srv.response.success)     
            ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]);
        // velocity初始速度设置为0   
        set_velocity_client = n->serviceClient<webots_ros::set_float>(string(ROBOT_NAME) + string(motorNames[i]) + string("/set_velocity"));   
        set_velocity_srv.request.value = 0.0;   
        if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
            ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]);   
        else     
            ROS_ERROR("Failed to call service set_vel