专栏首页机器人课程与技术Webots R2019和ROS使用笔记(机器人仿真软件与操作系统)

Webots R2019和ROS使用笔记(机器人仿真软件与操作系统)

安装Webots R2019后,启动:

选择喜欢的模式:

如果使用集成显卡会弹出一个警告,可以忽略:

参考教程目录进行学习。

编译对应ROS功能包,进行ROS学习:

启动webots和roscore后,然后使用rosrun尝试每个demo:

#include "ros/ros.h"
#include <signal.h>

#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>

#include <webots_ros/get_float.h>
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>

#include <webots_ros/save_image.h>
#include <webots_ros/robot_get_device_list.h>
#include <webots_ros/range_finder_get_info.h>

#define TIME_STEP 32;


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

ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;

// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr& name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.",controllerCount,controllerList.back().c_str());
}

// get range image from the range-finder
void rangeFinderCallback(const sensor_msgs::Image::ConstPtr& image) {
  int size = image->width * image->height;
  imageRangeFinder.resize(size);

  const float* depth_data = reinterpret_cast<const float*>(&image->data[0]);
  for (int i = 0; i < size; ++i)
    imageRangeFinder[i] = depth_data[i];
}

void quit(int sig) {
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ROS_INFO("User stopped the 'catch_the_bird' node.");
  ros::shutdown();
  exit(0);
}


int main(int argc, char **argv) {
  int wantedController = 0;
  std::string controllerName, motorName, rangeFinderName;
  std::vector<std::string> deviceList;
  int width,height;
  float i,step;
  bool birdCatched = false;

  if (argc != 1) {
    ROS_INFO("Usage: $ example_catch_bird.");
    return 1;
  }

  // create a node named 'catch_the_bird' on ROS network
  ros::init(argc, argv, "catch_the_bird",ros::init_options::AnonymousName);
  ros::NodeHandle n;

  signal(SIGINT,quit);

  // subscribe to the topic model_name to get the list of availables controllers
  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();

  // if there is more than one controller available, let the user choose
  if (controllerCount == 1)
    controllerName = controllerList[0];
  else {
    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;
    }
  }
  // leave topic once it's not necessary anymore
  nameSub.shutdown();

  // call device_list service to get the list of the devices available on the controller and print it
  // the device_list_srv object contains 2 members request and response. Their fields are described in the corresponding .srv file
  ros::ServiceClient deviceListClient = n.serviceClient<webots_ros::robot_get_device_list>(controllerName+"/robot/get_device_list");
  webots_ros::robot_get_device_list deviceListSrv;

  if (deviceListClient.call(deviceListSrv))
    deviceList = deviceListSrv.response.list;
  else
    ROS_ERROR("Failed to call service device_list.");
  motorName = deviceList[0];
  rangeFinderName = deviceList[1];

  ros::ServiceClient rangeFinderGetInfoClient = n.serviceClient<webots_ros::range_finder_get_info>(controllerName+'/'+rangeFinderName+"/get_info");
  webots_ros::range_finder_get_info rangeFinderGetInfoSrv;
  if (rangeFinderGetInfoClient.call(rangeFinderGetInfoSrv)) {
    width = rangeFinderGetInfoSrv.response.width;
    height = rangeFinderGetInfoSrv.response.height;
    ROS_INFO("Range-finder size is %d x %d.", width, height);
  } else
    ROS_ERROR("Failed to call service range_finder_get_info.");

  // enable the range-finder
  ros::ServiceClient enableRangeFinderClient = n.serviceClient<webots_ros::set_int>(controllerName+'/'+rangeFinderName+"/enable");
  webots_ros::set_int enableRangeFinderSrv;
  ros::Subscriber subRangeFinderRangeFinder;

  enableRangeFinderSrv.request.value = 2 * TIME_STEP;
  if (enableRangeFinderClient.call(enableRangeFinderSrv) && enableRangeFinderSrv.response.success) {
    ROS_INFO("Range-finder enabled with sampling period %d.",enableRangeFinderSrv.request.value);
    subRangeFinderRangeFinder = n.subscribe(controllerName+'/'+rangeFinderName+"/range_image",1,rangeFinderCallback);

    // wait for  the topics to be initialized
    while (subRangeFinderRangeFinder.getNumPublishers() == 0);
  } else
    ROS_ERROR("Failed to call service enable for %s.",rangeFinderName.c_str());

  ros::ServiceClient rangeFinderSaveImageClient = n.serviceClient<webots_ros::save_image>(controllerName+'/'+rangeFinderName+"/save_image");
  webots_ros::save_image rangeFinderSaveImageSrv;
  rangeFinderSaveImageSrv.request.filename = std::string(getenv("HOME")) + std::string("/bird_catched.png");
  rangeFinderSaveImageSrv.request.quality = 100;

  // enable motor
  ros::ServiceClient motorSetPositionClient = n.serviceClient<webots_ros::set_float>(controllerName+'/'+motorName+"/set_position");
  webots_ros::set_float motorSetPositionSrv;
  motorSetPositionSrv.request.value = 0;
  i = 0.2;
  step = 0.025;

  ros::ServiceClient motorGetTargetPositionClient = n.serviceClient<webots_ros::get_float>(controllerName+'/'+motorName+"/get_target_position");
  webots_ros::get_float motorGetTargetPositionSrv;

  // enable time_step
  timeStepClient = n.serviceClient<webots_ros::set_int>(controllerName+"/robot/time_step");
  timeStepSrv.request.value=TIME_STEP;

  // main loop
  while (!birdCatched && ros::ok()) {
    motorSetPositionSrv.request.value = i;
    motorSetPositionClient.call(motorSetPositionSrv);
    if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
      ROS_ERROR("Failed to call next step with time_step service.");
      exit(1);
    }
    motorGetTargetPositionClient.call(motorGetTargetPositionSrv);
    if (i >= 3.14)
      step = -0.025;
    if (i <= -3.14)
      step = 0.025;
    i += step;
    ros::spinOnce();
    while (imageRangeFinder.size() < (width * height))
      ros::spinOnce();
    // check if it sees the bird and take a picture of the bird
    if (imageRangeFinder[12 + (width * height / 4)] < 0.5) {
      birdCatched = true;
      if (rangeFinderSaveImageClient.call(rangeFinderSaveImageSrv) && rangeFinderSaveImageSrv.response.success == 1)
        ROS_INFO("What a beautifull bird we found here!");
      else
        ROS_INFO("Failed to call service save_image to take a picture of the bird.");
    }
  }
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  n.shutdown();
}

键盘遥控机器人在环境中运动。

#include "ros/ros.h"

#include <webots_ros/Int32Stamped.h>

#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>

#include <webots_ros/robot_get_device_list.h>

#include <std_msgs/String.h>

#include <signal.h>
#include <stdio.h>

#define TIME_STEP 32

static int controllerCount;
static std::vector<std::string> controllerList;
static std::string controllerName;
static double lposition = 0;
static double rposition = 0;


ros::ServiceClient leftWheelClient;
webots_ros::set_float leftWheelSrv;

ros::ServiceClient rightWheelClient;
webots_ros::set_float rightWheelSrv;

ros::ServiceClient timeStepClient;
webots_ros::set_int timeStepSrv;

ros::ServiceClient enableKeyboardClient;
webots_ros::set_int enableKeyboardSrv;


// catch names of the controllers availables on ROS network
void controllerNameCallback(const std_msgs::String::ConstPtr& name) {
  controllerCount++;
  controllerList.push_back(name->data);
  ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str());
}

void quit(int sig) {
  enableKeyboardSrv.request.value = 0;
  enableKeyboardClient.call(enableKeyboardSrv);
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ROS_INFO("User stopped the 'keyboard_teleop' node.");
  ros::shutdown();
  exit(0);
}

void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr& value) {
  int key = value->data;
  int send = 0;

  switch(key)
  {
    case 314:
      lposition += -0.2;
      rposition += 0.2;
      send = 1;
      break;
    case 316:
      lposition += 0.2;
      rposition += -0.2;
      send = 1;
      break;
    case 315:
      lposition += 0.2;
      rposition += 0.2;
      send = 1;
      break;
    case 317:
      lposition += -0.2;
      rposition += -0.2;
      send = 1;
      break;
    case 312:
      ROS_INFO("END.");
      quit(-1);
      break;
    default:
      send = 0;
      break;
  }

  leftWheelSrv.request.value = lposition;
  rightWheelSrv.request.value = rposition;
  if (send) {
    if (!leftWheelClient.call(leftWheelSrv) || !rightWheelClient.call(rightWheelSrv) || !leftWheelSrv.response.success || !rightWheelSrv.response.success)
      ROS_ERROR("Failed to send new position commands to the robot.");
  }
  return;
}


int main(int argc, char **argv) {
  int wantedController = 0;

  if (argc != 1) {
    ROS_INFO("Keyboard_teleop doesn't take any arguments.");
    return 1;
  }

  // create a node named 'keyboard_teleop' on ROS network
  ros::init(argc, argv, "keyboard_teleop",ros::init_options::AnonymousName);
  ros::NodeHandle n;

  signal(SIGINT,quit);

  // subscribe to the topic model_name to get the list of availables controllers
  ros::Subscriber nameSub = n.subscribe("model_name", 100, controllerNameCallback);
  while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) {
    ros::spinOnce();
    ros::spinOnce();
    ros::spinOnce();
  }
  ros::spinOnce();

  // if there is more than one controller available, let the user choose
  if (controllerCount == 1)
    controllerName = controllerList[0];
  else {
    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;
    }
  }
  // leave topic once it's not necessary anymore
  nameSub.shutdown();

  leftWheelClient = n.serviceClient<webots_ros::set_float>(controllerName+"/left_wheel/set_position");
  rightWheelClient = n.serviceClient<webots_ros::set_float>(controllerName+"/right_wheel/set_position");
  timeStepClient = n.serviceClient<webots_ros::set_int>(controllerName+"/robot/time_step");
  timeStepSrv.request.value = TIME_STEP;

  enableKeyboardClient = n.serviceClient<webots_ros::set_int>(controllerName+"/keyboard/enable");
  enableKeyboardSrv.request.value = TIME_STEP;
  if (enableKeyboardClient.call(enableKeyboardSrv) && enableKeyboardSrv.response.success) {
    ros::Subscriber sub_keyboard;
    sub_keyboard = n.subscribe(controllerName+"/keyboard/key", 1, keyboardCallback);
    while (sub_keyboard.getNumPublishers() == 0);
    ROS_INFO("Keyboard enabled.");
    ROS_INFO("Use the arrows in Webots window to move the robot.");
    ROS_INFO("Press the End key to stop the node.");

    // main loop
    while (ros::ok()) {
      ros::spinOnce();
      if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success)
        ROS_ERROR("Failed to call service time_step for next step.");
    }
  }
  else
    ROS_ERROR("Could not enable keyboard, success = %d.", enableKeyboardSrv.response.success);

  enableKeyboardSrv.request.value = 0;
  if (!enableKeyboardClient.call(enableKeyboardSrv) || !enableKeyboardSrv.response.success)
    ROS_ERROR("Could not disable keyboard, success = %d.", enableKeyboardSrv.response.success);
  timeStepSrv.request.value = 0;
  timeStepClient.call(timeStepSrv);
  ros::shutdown();
  return(0);
}

e-puck巡线程序。


本文参与腾讯云自媒体分享计划,欢迎正在阅读的你也加入,一起分享。

我来说两句

0 条评论
登录 后参与评论

相关文章

  • Ubuntu与ROS的Docker桌面系统与ROS在线练习课程(在线Linux虚拟机)

    https://www.shiyanlou.com/courses/854 邀请码 U23ERF8H

    zhangrelay
  • ROS机器人程序设计(原书第2版)补充资料 (肆) 第四章 在ROS下使用传感器和执行器

    ROS机器人程序设计(原书第2版)补充资料 (肆) 第四章 在ROS使用传感器和执行器

    zhangrelay
  • ROS机器人项目开发11例-ROS Robotics Projects(8)自动驾驶汽车

    自动驾驶汽车近年来非常热门,也是本书第10章的内容,优酷小站也分享过很多这方面主题的视频。

    zhangrelay
  • https://github.com/CPFL/Autoware 自动驾驶框架比较齐全

    Integrated open-source software for urban autonomous driving, maintained by Tier...

    用户1908973
  • ROS机器人程序设计(原书第2版)补充资料 (肆) 第四章 在ROS下使用传感器和执行器

    ROS机器人程序设计(原书第2版)补充资料 (肆) 第四章 在ROS使用传感器和执行器

    zhangrelay
  • ROS | 无法锁定管理目录(/var/lib/dpkg/),是否有其他进程正占用它

    以上是一句安装ros一个插件的语句, 运行时需要获得锁 /var/lib/dpkg/lock; 没有获得锁时,会出现“无法锁定管理目录(/var/lib/d...

    凌川江雪
  • 热修复框架?我们都能做出来!

    大家都是开发,所以应该都知道有一个东西我们永远也避免不了。不错,Bug!我们在开发阶段碰到bug那还好,直接解决就是了,大不了让测试多测一轮。可是,如果我们发出...

    吴延宝
  • 数据库语句练习

    (31)在Visual FoxPro中,以下有关SQL的SELECT语句的叙述中,错误的是______。 A.SELECT子句中可以包含表中的列和表达式 B...

    阳光岛主
  • [设计模式] 命令模式

    假定我们定义一个宏命令实现回家后的一个智能操作:包含开灯和开电视,并要求它支持撤销能力。

    架构探险之道
  • 设计模式的征途—15.观察者(Observer)模式

    在日常生活中,交通信号灯指挥者日益拥挤的城市交通。红灯亮,汽车停止;绿灯亮,汽车继续前行;在这个过程中,交通信号灯是汽车的观察目标,而汽车则是观察者。随着交通信...

    Edison Zhou

扫码关注云+社区

领取腾讯云代金券