新建环境模型

1 控制器结构
2 操作函数
- 2.1 初始化函数
- 2.2 距离传感器操作函数
- 2.3 ground_sensors 操作函数
- 2.4 激光雷达传感器操作函数
- 2.5 LED灯作函数
- 2.7 电机操作函数
3 DEMO
参考资料

这里我们分析webots中的基础控制器，这是由于这其中涉及到很多的基础控制函数（比如读取传感器数据、控制电机），通过了解这些基础控制器中的操作函数，为我们后面介绍ROS控制做基础。

这里以webots自带的机器人 e-puck 的控制器为例，介绍webots控制器的编写规范。
首先我们打开 e-puck_line.wbt 模型文件，选择自带的避障控制器，如下图：

1 控制器结构

webots的控制器和C语言的语法非常相似，入口地址为函数main()函数，webots访问机器人上的每一个传感器（距离传感器）和执行器（电机）都是依靠操作句柄实现的，操作句柄可以被理解为一个结构体，每一个传感器都对应一个操作句柄，在创建这个操作句柄的时候需要输入这个传感器的名称。

/* include headers */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>#include <webots/device.h>
#include <webots/distance_sensor.h>
#include <webots/led.h>
#include <webots/motor.h>
#include <webots/nodes.h>
#include <webots/robot.h>/* Device stuff */
#define DISTANCE_SENSORS_NUMBER 8
static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER];
static double distance_sensors_values[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {"ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"};#define GROUND_SENSORS_NUMBER 3
static WbDeviceTag ground_sensors[GROUND_SENSORS_NUMBER];
static double ground_sensors_values[GROUND_SENSORS_NUMBER] = {0.0, 0.0, 0.0};
static const char *ground_sensors_names[GROUND_SENSORS_NUMBER] = {"gs0", "gs1", "gs2"};#define LEDS_NUMBER 10
static WbDeviceTag leds[LEDS_NUMBER];
static bool leds_values[LEDS_NUMBER];
static const char *leds_names[LEDS_NUMBER] = {"led0", "led1", "led2", "led3", "led4", "led5", "led6", "led7", "led8", "led9"};static WbDeviceTag left_motor, right_motor;#define LEFT 0
#define RIGHT 1
#define MAX_SPEED 6.28
static double speeds[2];/* Breitenberg stuff */
static double weights[DISTANCE_SENSORS_NUMBER][2] = {{-1.3, -1.0}, {-1.3, -1.0}, {-0.5, 0.5}, {0.0, 0.0},{0.0, 0.0},   {0.05, -0.5}, {-0.75, 0},  {-0.75, 0}};
static double offsets[2] = {0.5 * MAX_SPEED, 0.5 * MAX_SPEED};static int get_time_step() {static int time_step = -1;if (time_step == -1)time_step = (int)wb_robot_get_basic_time_step();return time_step;
}static void step() {if (wb_robot_step(get_time_step()) == -1) {wb_robot_cleanup();exit(EXIT_SUCCESS);}
}static void passive_wait(double sec) {double start_time = wb_robot_get_time();do {step();} while (start_time + sec > wb_robot_get_time());
}static void init_devices() {int i;for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {distance_sensors[i] = wb_robot_get_device(distance_sensors_names[i]);wb_distance_sensor_enable(distance_sensors[i], get_time_step());}for (i = 0; i < LEDS_NUMBER; i++)leds[i] = wb_robot_get_device(leds_names[i]);// silently initialize the ground sensors if they existsfor (i = 0; i < GROUND_SENSORS_NUMBER; i++)ground_sensors[i] = (WbDeviceTag)0;int ndevices = wb_robot_get_number_of_devices();for (i = 0; i < ndevices; i++) {WbDeviceTag dtag = wb_robot_get_device_by_index(i);const char *dname = wb_device_get_name(dtag);WbNodeType dtype = wb_device_get_node_type(dtag);if (dtype == WB_NODE_DISTANCE_SENSOR && strlen(dname) == 3 && dname[0] == 'g' && dname[1] == 's') {int id = dname[2] - '0';if (id >= 0 && id < GROUND_SENSORS_NUMBER) {ground_sensors[id] = wb_robot_get_device(ground_sensors_names[id]);wb_distance_sensor_enable(ground_sensors[id], get_time_step());}}}// get a handler to the motors and set target position to infinity (speed control).left_motor = wb_robot_get_device("left wheel motor");right_motor = wb_robot_get_device("right wheel motor");wb_motor_set_position(left_motor, INFINITY);wb_motor_set_position(right_motor, INFINITY);wb_motor_set_velocity(left_motor, 0.0);wb_motor_set_velocity(right_motor, 0.0);step();
}static void reset_actuator_values() {int i;for (i = 0; i < 2; i++)speeds[i] = 0.0;for (i = 0; i < LEDS_NUMBER; i++)leds_values[i] = false;
}static void get_sensor_input() {int i;for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {distance_sensors_values[i] = wb_distance_sensor_get_value(distance_sensors[i]);// scale the data in order to have a value between 0.0 and 1.0// 1.0 representing something to avoid, 0.0 representing nothing to avoiddistance_sensors_values[i] /= 4096;}for (i = 0; i < GROUND_SENSORS_NUMBER; i++) {if (ground_sensors[i])ground_sensors_values[i] = wb_distance_sensor_get_value(ground_sensors[i]);}
}static bool cliff_detected() {int i;for (i = 0; i < GROUND_SENSORS_NUMBER; i++) {if (!ground_sensors[i])return false;if (ground_sensors_values[i] < 500.0)return true;}return false;
}static void set_actuators() {int i;for (i = 0; i < LEDS_NUMBER; i++)wb_led_set(leds[i], leds_values[i]);wb_motor_set_velocity(left_motor, speeds[LEFT]);wb_motor_set_velocity(right_motor, speeds[RIGHT]);
}static void blink_leds() {static int counter = 0;counter++;leds_values[(counter / 10) % LEDS_NUMBER] = true;
}static void run_braitenberg() {int i, j;for (i = 0; i < 2; i++) {speeds[i] = 0.0;for (j = 0; j < DISTANCE_SENSORS_NUMBER; j++)speeds[i] += distance_sensors_values[j] * weights[j][i];speeds[i] = offsets[i] + speeds[i] * MAX_SPEED;if (speeds[i] > MAX_SPEED)speeds[i] = MAX_SPEED;else if (speeds[i] < -MAX_SPEED)speeds[i] = -MAX_SPEED;}
}static void go_backwards() {wb_motor_set_velocity(left_motor, -MAX_SPEED);wb_motor_set_velocity(right_motor, -MAX_SPEED);passive_wait(0.2);
}static void turn_left() {wb_motor_set_velocity(left_motor, -MAX_SPEED);wb_motor_set_velocity(right_motor, MAX_SPEED);passive_wait(0.2);
}int main(int argc, char **argv) {wb_robot_init();printf("Default controller of the e-puck robot started...\n");init_devices();while (true) {reset_actuator_values();get_sensor_input();blink_leds();if (cliff_detected()) {go_backwards();turn_left();} else {run_braitenberg();}set_actuators();step();};return EXIT_SUCCESS;
}

2 操作函数

首先我们来看看上面文件中的主要操作函数

2.1 初始化函数

webots的初始化函数为：

wb_robot_init();

2.2 距离传感器操作函数

1、e-puck有着8个距离传感器名字分别为（“ps0”, “ps1”, “ps2”, “ps3”, “ps4”, “ps5”, “ps6”, “ps7”）

#define DISTANCE_SENSORS_NUMBER 8
static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER]; // 申明数组
static double distance_sensors_values[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {"ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"};

2、构建操作句柄，使能传感器

 int i;for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {distance_sensors[i] = wb_robot_get_device(distance_sensors_names[i]);wb_distance_sensor_enable(distance_sensors[i], get_time_step());}

3、读取传感器的数据

  int i;for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {distance_sensors_values[i] = wb_distance_sensor_get_value(distance_sensors[i]);// scale the data in order to have a value between 0.0 and 1.0// 1.0 representing something to avoid, 0.0 representing nothing to avoiddistance_sensors_values[i] /= 4096;}

2.3 ground_sensors 操作函数

1、初始化变量

#define GROUND_SENSORS_NUMBER 3
static WbDeviceTag ground_sensors[GROUND_SENSORS_NUMBER];
static double ground_sensors_values[GROUND_SENSORS_NUMBER] = {0.0, 0.0, 0.0};
static const char *ground_sensors_names[GROUND_SENSORS_NUMBER] = {"gs0", "gs1", "gs2"};

2、构建操作句柄，使能传感器

 ground_sensors[id] = wb_robot_get_device(ground_sensors_names[id]);wb_distance_sensor_enable(ground_sensors[id], get_time_step());

3、读取传感器的数据

for (i = 0; i < GROUND_SENSORS_NUMBER; i++) {if (ground_sensors[i])ground_sensors_values[i] = wb_distance_sensor_get_value(ground_sensors[i]);}

2.4 激光雷达传感器操作函数

2.5 LED灯作函数

1、声明变量

#define LEDS_NUMBER 10
static WbDeviceTag leds[LEDS_NUMBER];
static bool leds_values[LEDS_NUMBER];
static const char *leds_names[LEDS_NUMBER] = {"led0", "led1", "led2", "led3", "led4", "led5", "led6", "led7", "led8", "led9"};

2、构建操作句柄

  for (i = 0; i < LEDS_NUMBER; i++)leds[i] = wb_robot_get_device(leds_names[i]);

3、设置LED的值

  for (i = 0; i < LEDS_NUMBER; i++)wb_led_set(leds[i], leds_values[i]);

2.7 电机操作函数

1、获取电机操作句柄。"left wheel motor"表示电机的名称

  left_motor = wb_robot_get_device("left wheel motor"); right_motor = wb_robot_get_device("right wheel motor");

2、设置电机行进的位移

  wb_motor_set_position(left_motor, INFINITY);wb_motor_set_position(right_motor, INFINITY);

3、设置电机速度

  wb_motor_set_velocity(left_motor, 0.0);wb_motor_set_velocity(right_motor, 0.0);

从上面的形式来看，每一个传感器操作都会经历声明变量、构建操作句柄、获取传感器值三个操作，其中获取操作句柄都是使用的 wb_robot_get_device（）这个函数实现的。

3 DEMO

下面是我在【2】上修改到的一个控制，读取距离传感器的值实现小车避障，同时小车身上的灯会循环点亮。

以下是c文件的控制器代码

#include <webots/robot.h>
#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include <webots/led.h>
// time in [ms] of a simulation step
#define TIME_STEP 64#define MAX_SPEED 6.28/* Device stuff */
#define DISTANCE_SENSORS_NUMBER 8
static WbDeviceTag distance_sensors[DISTANCE_SENSORS_NUMBER];
static double distance_sensors_values[DISTANCE_SENSORS_NUMBER];
static const char *distance_sensors_names[DISTANCE_SENSORS_NUMBER] = {"ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"};#define LEDS_NUMBER 10
static WbDeviceTag leds[LEDS_NUMBER];
static bool leds_values[LEDS_NUMBER];
static const char *leds_names[LEDS_NUMBER] = {"led0", "led1", "led2", "led3", "led4", "led5", "led6", "led7", "led8", "led9"};static int counter = 0;
// entry point of the controller
int main(int argc, char **argv) {// initialize the Webots APIwb_robot_init();// internal variablesint i;for (i = 0; i < DISTANCE_SENSORS_NUMBER; i++) {distance_sensors[i] = wb_robot_get_device(distance_sensors_names[i]);wb_distance_sensor_enable(distance_sensors[i], TIME_STEP);}for (i = 0; i < LEDS_NUMBER; i++)leds[i] = wb_robot_get_device(leds_names[i]);WbDeviceTag left_motor = wb_robot_get_device("left wheel motor");WbDeviceTag right_motor = wb_robot_get_device("right wheel motor");wb_motor_set_position(left_motor, INFINITY);wb_motor_set_position(right_motor, INFINITY);wb_motor_set_velocity(left_motor, 0.0);wb_motor_set_velocity(right_motor, 0.0);// feedback loop: step simulation until an exit event is receivedwhile (wb_robot_step(TIME_STEP) != -1) {// read sensors outputsfor (i = 0; i < DISTANCE_SENSORS_NUMBER ; i++)distance_sensors_values[i] = wb_distance_sensor_get_value(distance_sensors[i]);// detect obstaclesbool right_obstacle =distance_sensors_values[0] > 80.0 ||distance_sensors_values[1] > 80.0 ||distance_sensors_values[2] > 80.0;bool left_obstacle =distance_sensors_values[5] > 80.0 ||distance_sensors_values[6] > 80.0 ||distance_sensors_values[7] > 80.0;// initialize motor speeds at 50% of MAX_SPEED.double left_speed  = 0.5 * MAX_SPEED;double right_speed = 0.5 * MAX_SPEED;// modify speeds according to obstaclesif (left_obstacle) {// turn rightleft_speed  += 0.5 * MAX_SPEED;right_speed -= 0.5 * MAX_SPEED;}else if (right_obstacle) {// turn leftleft_speed  -= 0.5 * MAX_SPEED;right_speed += 0.5 * MAX_SPEED;}for (i = 0; i < LEDS_NUMBER; i++)leds_values[i] = false;counter++;leds_values[(counter / 10) % LEDS_NUMBER] = true;for (i = 0; i < LEDS_NUMBER; i++)wb_led_set(leds[i], leds_values[i]);// write actuators inputswb_motor_set_velocity(left_motor, left_speed);wb_motor_set_velocity(right_motor, right_speed);}// cleanup the Webots APIwb_robot_cleanup();return 0; //EXIT_SUCCESS
}

参考资料

[1] https://www.cyberbotics.com/doc/reference/motion
[2] https://cyberbotics.com/doc/guide/tutorial-4-more-about-controllers?tab-language=c

如果大家觉得文章对你有所帮助，麻烦大家帮忙点个赞。O(∩_∩)O

欢迎大家在评论区交流讨论（cenruping@vip.qq.com）

上一篇：Webot机器人仿真平台(三) 新建环境模型下一篇：Webot机器人仿真平台(五) 新建四轮小车模型

Webots 机器人仿真平台(四) 机器人基础控制器相关推荐

基于C#的机器人仿真平台和机器人运动学算法实现
一.平台搭建 1.利用wpf自带的库进行机器人各关节导入相关代码段: <Grid.ColumnDefinitions><ColumnDefinition Width="0 ...
Webots 机器人仿真平台(五) 新建四轮小车模型
新建机器人模型 1 添加机器人本体 1.1 添加Body 1.2 添加机器人轮子 2 添加距离传感器 3 小结参考资料下图为我们这篇博客创建的小车效果,接下来我们一步一步的实现在webots中搭建 ...
Webots 机器人仿真平台(三) 新建环境模型
新建环境模型 1 新建世界模型 2 添加机器人模型 3 添加机器人控制器参考资料这篇博客介绍如何在webot中新建一个世界并在该环境中加入障碍物.机器人等元素,并添加机器人对应的控制器.这可以帮助 ...
Webots 机器人仿真平台(六) 新建四轮小车控制器
新建机器人控制器 1 新建控制器 2 控制器说明 3 运行效果参考资料 1 新建控制器这里我们为上一篇博客中创建的小车模型建立一个控制器,让小车在环境中避障运行.首先在 Wizards->N ...
Webots 机器人仿真平台(十二) 与ROS节点通讯
与ROS节点通讯 1 设置webots控制器 2 创建ROS节点 3 代码分析 3.1 设置电机位置 3.2 设置电机速度 3.3 读取时间节拍参考资料在前面的教程中我们描述了如何在webots中 ...
Webots 机器人仿真平台(二) 与ROS通讯
Webot 与ROS通讯 1 Webot Ubuntu客户端与ROS通讯 1.1 配置webots_ros包 1.2 启动webots_ros包 1.3 gmapping建图测试 1.4 键盘遥控we ...
Webots 机器人仿真平台(十) 添加camera相机
添加camera相机 1. 添加camera实体 2. 添加camera控制接口代码 3. 运行效果参考资料 1. 添加camera实体 step1: 在Robot中添加 camera(camera ...
Webots 机器人仿真平台(一) 系统安装
系统安装 1 Webot介绍 2 Webot安装 2.1 ubuntu环境安装 2.2 Windows环境安装 3 参考资料 1 Webot介绍 Webots是一个开源的三维移动机器人模拟器,它与ga ...
干货 | 浅谈机器人仿真平台
" 仿真是机器人学习和研究过程中最重要的工具之一,是指通过计算机对实际的物理系统进行模拟的技术.机器人仿真基于交互式计算机图形技术和机器人学理论,生成机器人的几何图形,并对其进行三维显示,用 ...

Webots 机器人仿真平台(四) 机器人基础控制器