1. 添加GPS實體
step1: 首先在機器人模型的Robot->children中添加一個GPS節點
step2: 然後在GPS節點->children中添加一個solid固件
step3: 設置這個solid固件的children中添加shape節點,並設置外觀和形狀。具體設置底部半徑爲0.02 高度爲0.05,設置偏移量爲(x=0,y=0.03,z=0)。
這裏爲了區別其他傳感器我們使用了這個圓錐形的形狀作爲GPS
step4: 最後我們需要設置GPS傳感器的名稱,以便我們在程序中讀取GPS傳感器的數據。
注意:添加GPS的過程中不設置boundingObject屬性和 physics屬性。
2. 添加GPS 控制接口代碼
完整的代碼塊:
#include <webots/Robot.hpp>
#include <webots/GPS.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>
#include <webots/Keyboard.hpp>
#include <stdio.h>
#define TIME_STEP 64
// All the webots classes are defined in the "webots" namespace
using namespace webots;
int main(int argc, char **argv) {
// create the Robot instance.
Robot *robot = new Robot();
Keyboard kb;
DistanceSensor *ds[2];
char dsNames[2][10] = {"ds_right","ds_left"};
for (int i = 0; i < 2; i++) {
ds[i] = robot->getDistanceSensor(dsNames[i]);
ds[i]->enable(TIME_STEP);
}
GPS *gps;
gps = robot->getGPS("global_gps");
gps->enable(TIME_STEP);
// initialise motors
Motor *wheels[4];
char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
for (int i = 0; i < 4; i++) {
wheels[i] = robot->getMotor(wheels_names[i]);
wheels[i]->setPosition(INFINITY);
wheels[i]->setVelocity(0);
}
printf("init successd ...\n");
kb.enable(TIME_STEP);
double leftSpeed = 0.0;
double rightSpeed = 0.0;
// Main loop:
// - perform simulation steps until Webots is stopping the controller
while (robot->step(TIME_STEP) != -1)
{
int key = kb.getKey();
if(key== 315)
{
leftSpeed = 3.0;
rightSpeed = 3.0;
}
else if(key== 317)
{
leftSpeed = -3.0;
rightSpeed = -3.0;
}
else if(key== 314)
{
leftSpeed = -3.0;
rightSpeed = 3.0;
}
else if(key== 316)
{
leftSpeed = 3.0;
rightSpeed = -3.0;
}
else
{
leftSpeed = 0.0;
rightSpeed = 0.0;
}
std::cout<< " Right Sensor Value:" <<ds[0]->getValue() << " Left Sensor Value:" <<ds[1]->getValue() <<std::endl;
std::cout<< "GPS Value X: " <<gps->getValues()[0]
<< " Y: " <<gps->getValues()[1]<< " Z: " <<gps->getValues()[2] <<std::endl;
wheels[0]->setVelocity(leftSpeed);
wheels[1]->setVelocity(rightSpeed);
wheels[2]->setVelocity(leftSpeed);
wheels[3]->setVelocity(rightSpeed);
};
// Enter here exit cleanup code.
delete robot;
return 0;
}
相比於利用鍵盤控制小車的demo,在這裏增加了GPS初始化和打印GPS信息兩個部分。
在控制器中增加代碼塊,用於初始化GPS
GPS *gps;
gps = robot->getGPS("global_gps");
gps->enable(TIME_STEP);
打印GPS傳感器的值
std::cout<< "GPS Value X: " <<gps->getValues()[0]
<< " Y: " <<gps->getValues()[1]<< " Z: " <<gps->getValues()[2] <<std::endl;
GPS傳感器的接口函數可以在[1]處查到各語言版本的接口類型,這裏我們只用到了 enable 和 getValues 兩個函數。
3. 運行效果
在console框中可以看到輸出的GPS信息,GPS安裝的有點難看,美化的工作就留給大家了。
修改的模型文件可在此處下載
:
參考資料
[1] https://cyberbotics.com/doc/reference/gps?tab-language=c++
[2] 模型文件:https://download.csdn.net/download/crp997576280/12351539
如果大家覺得文章對你有所幫助,麻煩大家幫忙點個贊。O(∩_∩)O
歡迎大家在評論區交流討論([email protected])