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_velocity on motor %s.", motorNames[i]);
    }

这些就是冗余代码，这么一长串程序，目的都是同一个，为什么不把他写成头文件呢，不但美观了源程序，还提高了工作效率。

Webots_ros头文件

在webots_ros头文件中提供了常用的代码，将他们封装在了函数中，方便用户调用。

class Webots
{
private:
    // 同步Webots所设置的时钟
    int TIME_STEP;
    // 机器人的名称，对应Webots中Robot下的name属性
    std::string ROBOT_NAME;
public:
    // 时钟通讯service客户端
    ros::ServiceClient timeStepClient;  
    // 时钟服务数据        
    webots_ros::set_int timeStepSrv;            
    // Webots类构造函数，设置必须变量
    Webots(int TIME_STEP,std::string ROBOT_NAME);
    // 初始化Webots和Ros之间的桥梁，成功返回0，失败返回1
    int Init(ros::NodeHandle *n, ros::Subscriber nameSub, const int &controllerCount, const std::vector<std::string> &controllerList);
    // 初始化电机，成功返回0，失败返回1
    int InitMotors(ros::NodeHandle *n, const char *motorNames[], int NMOTORS); 
    // 设置电机位置属性，成功返回0，失败返回1
    int SetMotorsPositon(ros::NodeHandle *n, const char *motorNames, float value);
    // 设置电机速度属性，成功返回0，失败返回1
    int SetMotorsVelocity(ros::NodeHandle *n, const char *motorNames, float value);
    // 使能Webots服务，成功返回0，失败返回1
    int EnableService(ros::NodeHandle *n, std::string Service_name);
    // 检测时钟通讯，成功返回0，失败返回1
    int ChecktimeStep();
    // 退出函数
    void Quit();
};

1. Init函数

 std::string controllerName;
// 订阅webots中所有可用的model_name
while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
    ROS_INFO("controllerCount=%d",controllerCount);
    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();

2. InitMotors函数

for (int i = 0; i < NMOTORS; ++i) {
    SetMotorsPositon(n, motorNames[i], INFINITY);
    SetMotorsVelocity(n,motorNames[i], 0.0);
}

3. SetMotorsPositon函数

ros::ServiceClient set_position_client;     // 电机位置通讯service客户端
webots_ros::set_float set_position_srv;     // 电机位置服务数据
set_position_client = n->serviceClient<webots_ros::set_float>(ROBOT_NAME + "/" + std::string(motorNames) + std::string("/set_position"));   
set_position_srv.request.value = value;
if (set_position_client.call(set_position_srv) && set_position_srv.response.success){
    ROS_INFO("Position set to %f for motor %s.", value, motorNames);   
    return 0;
}   
else{
    ROS_ERROR("Failed to call service set_position on motor %s.", motorNames);
    return 1;
}

4. SetMotorsVelocity函数

ros::ServiceClient set_velocity_client;     // 电机速度通讯service客户端
webots_ros::set_float set_velocity_srv;     // 电机速度服务数据 
set_velocity_client = n->serviceClient<webots_ros::set_float>(ROBOT_NAME + "/" + std::string(motorNames) + std::string("/set_velocity"));   
set_velocity_srv.request.value = value;   
if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1)     
    ROS_INFO("Velocity set to %f for motor %s.",value, motorNames);   
else     
    ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames); 

5. EnableService函数

ros::ServiceClient EnableClient;
webots_ros::set_int Enablesrv;

EnableClient = n->serviceClient<webots_ros::set_int>(ROBOT_NAME + "/" + Service_name + "/enable");
Enablesrv.request.value = TIME_STEP;
if (EnableClient.call(Enablesrv) && Enablesrv.response.success){
    ROS_INFO("Enable %s successful", Service_name);
    return 0;
} 
else{
    ROS_ERROR("Could not enable %s, success = %d.", Service_name, Enablesrv.response.success);
    return 1;
}

6. ChecktimeStep函数

if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success){  
    ROS_ERROR("Failed to call service time_step for next step.");     
    return 1;   
}else
    return 0;

7. Quit函数

ROS_INFO("Stopped the node.");
timeStepSrv.request.value = 0; 
delete n;
timeStepClient.call(timeStepSrv); 
ros::shutdown();
exit(0);

现在的控制程序

1. 将define修改为const
2. 引用了webots_ros.h去除webots的头文件
3. 将上面的冗余代码全部替换成Webots类
4. 时钟检测，使能传感器也能通过webots_ros实现
5. 电机速度通过webots_ros封装的代码实现

通过webots_ros头文件可以为原来的程序省下80-100行冗余代码。

/************************************************* 
Copyright:Webots Demo
Author: 锡城筱凯
Date:2021-06-30 
Blog：https://blog.csdn.net/xiaokai1999
Change: 2021-11-07
Description:Webots Demo 通过webots控制机器人移动
**************************************************/  
#include <signal.h>
#include <locale.h> 
#include <webots_ros.h>
#include <nav_msgs/Odometry.h>

ros::NodeHandle *n;

const int TIME_STEP = 32;                   // 时钟
const int NMOTORS = 2;                      // 电机数量
const float MAX_SPEED = 2.0;                // 电机最大速度
const std::string ROBOT_NAME = "robot/";    // ROBOT名称 
double speeds[NMOTORS]={0.0,0.0};           // 电机速度值 0.0～10.0
float linear_temp=0, angular_temp=0;        // 暂存的线速度和角速度

static const char *motorNames[NMOTORS] ={"left_motor", "right_motor"};// 控制位置电机名称

static int controllerCount;
static std::vector<std::string> controllerList; 

ros::Publisher pub_speed;                   // 发布 /vel
Webots w = Webots(TIME_STEP,ROBOT_NAME);

/*******************************************************
* Function name ：updateSpeed
* Description   ：将速度请求以set_float的形式发送给set_velocity_srv
* Parameter     ：无
* Return        ：无
**********************************************************/
void updateSpeed() {   
    nav_msgs::Odometry speed_data;
    //两轮之间的距离
    float L = 0.6;
    speeds[0]  = 10.0*(2.0*linear_temp - L*angular_temp)/2.0;
    speeds[1]  = 10.0*(2.0*linear_temp + L*angular_temp)/2.0;
    for (int i = 0; i < NMOTORS; ++i) {
        // 更新速度
        w.SetMotorsVelocity(n, motorNames[i], -speeds[i]);
    }
    speed_data.header.stamp = ros::Time::now();
    speed_data.twist.twist.linear.x = linear_temp;
    speed_data.twist.twist.angular.z = angular_temp;
    pub_speed.publish(speed_data);
    speeds[0]=0;
    speeds[1]=0;
}

/*******************************************************
* Function name ：controllerNameCallback
* Description   ：控制器名回调函数，获取当前ROS存在的机器人控制器
* Parameter     ：
        @name   控制器名
* Return        ：无
**********************************************************/
void controllerNameCallback(const std_msgs::String::ConstPtr &name) { 
    controllerCount++; 
    controllerList.push_back(name->data);//将控制器名加入到列表中
    ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

/*******************************************************
* Function name ：键盘返回函数
* Description   ：当键盘动作，就会进入此函数内
* Parameter     ：
        @value   返回的值
* Return        ：无
**********************************************************/
void keyboardDataCallback(const webots_ros::Int32Stamped::ConstPtr &value)
{
    switch (value->data){
        // 左转
        case 314:
            angular_temp-=0.1;
            break;
        // 前进
        case 315:
            linear_temp += 0.1;
            break;
        // 右转
        case 316:
            angular_temp+=0.1;
            break;
        // 后退
        case 317:
            linear_temp-=0.1;
            break;
        // 停止
        case 32:
            linear_temp = 0;
            angular_temp = 0;
            break;
        default:
            break;
    }
}

/*******************************************************
* Function name ：quit
* Description   ：退出函数
* Parameter     ：
        @sig   信号
* Return        ：无
**********************************************************/
void quit(int sig) {
    w.Quit(n);
}
int main(int argc, char **argv) {
    std::string controllerName;
    // 在ROS网络中创建一个名为robot_init的节点
    ros::init(argc, argv, "robot_init", ros::init_options::AnonymousName);
    n = new ros::NodeHandle;
    // 截取退出信号
    signal(SIGINT, quit);

    // 订阅webots中所有可用的model_name
    ros::Subscriber nameSub = n->subscribe("model_name", 10, controllerNameCallback);
    w.Init(n, nameSub, controllerCount, controllerList);
    w.InitMotors(n, motorNames, NMOTORS);
    pub_speed = n->advertise<nav_msgs::Odometry>("/vel",1);
    if(!w.EnableService(n, "keyboard")){
        ros::Subscriber keyboardSub;
        keyboardSub = n->subscribe(std::string(ROBOT_NAME)+std::string("keyboard/key"),1,keyboardDataCallback);
        while (keyboardSub.getNumPublishers() == 0) {}
        setlocale(LC_CTYPE,"zh_CN.utf8");//设置中文
        ROS_INFO("Keyboard enabled.");
        ROS_INFO("控制方向：");
        ROS_INFO("  ↑  ");
        ROS_INFO("← ↓ →");
        ROS_INFO("刹车：空格键");
        ROS_INFO("Use the arrows in Webots window to move the robot.");
        ROS_INFO("Press the End key to stop the node.");
        while (ros::ok()) {   
            ros::spinOnce();
            updateSpeed();
            if (w.ChecktimeStep())break;    
            ros::spinOnce();
        } 
    }
    return 0;
}

如何安装

下载地址

1. 例如webots_demo，在webots_demo下新建一个include文件夹。

2. 将webots_ros.h复制到include文件夹下.

3. 配置修改CMakeList.txt，将

include_directories(
  #include
  ${catkin_INCLUDE_DIRS}
)

修改为：去除include前的#号

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

4. 编译程序后就能使用webots_ros.h了

程序测试效果

程序肯定有很多bug，希望大家能够提出，大家共同进步^.