ROS联合webots实战案例(五)导航功能包入门1

166
0
2021年1月23日 09时18分

导航功能包入门1

 

注意:

 

  • 再学习本系列教程时,应该已经安装过ROS了并且需要有一些ROS的基本知识

webots版本:2020b rev1
ros版本:melodic

 

在前面几章中分别介绍了在webots中如何创建自己的机器人、添加传感器以及使用手柄或键盘驱动它在仿真环境中移动。在本章中,你会学习到ROS系统最强大的特性之一,它能够让你的机器人自主导航和运动。

 

1. ROS导航框架

 

ROS联合webots实战案例(五)导航功能包入门1插图

 

在图中,能够看到白色、灰色和虚线三种框。白框表示其中的这些功能包集已经在ROS中集成了,并且它们提供的多种节点能够为机器人实现自主导航。

 

2. 测量或估计机器人姿态

 

在webots中可以直接使用GPS进行定位。并且利用IMU传感器获取惯性信息来补偿位置和方向值。

 

姿态(位置+方向):在ROS中,机器人的位置(position:x,y,z)和方向(orientation:x,y,z,w)被定义为姿态。

 

2.1 在webots中加入GPS和IMU

 

  1. 打开webots
  2. 在Robot->children下添加如下两个设备
    ROS联合webots实战案例(五)导航功能包入门1插图(1)
  3. 保存并刷新场景
  4. 在控制台下输入以下命令查看是否同步到webots

 

$ rosservice list

可以看到已经同步上来了

 

ROS联合webots实战案例(五)导航功能包入门1插图(2)

 

3. 识别障碍物

 

这里我们使用了激光雷达,在上一节已经带大家配置、调试好了。
在webots中包含了市面上常见的传感器。有距离传感器和视觉传感器等多种传感器。其中距离传感器有基于雷达的距离传感器(常用的是LDS、LRF和LiDAR)、超声波传感器和红外距离传感器等,而视觉传感器包括立体相机、单镜相机、360度相机,以及经常用作深度摄像头的Kinect也都用于识别障碍物。

 

4. 创建变换

 

导航包需要知道传感器、轮子和关节的位置。
在这里我们使用tf软件库来完成这部分工作。它会管理坐标变换树。

 

4.1 创建广播机构

 

  1. 让我们创建一个代码测试测试一下。在webots_demo/src文件夹下创建一个robot_broadcaster.cpp
  2. 为了不重复造轮子,直接把webots_ros基础代码复制进来。

 

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

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

using namespace std;
#define TIME_STEP 32    //时钟
#define NMOTORS 2       //电机数量
#define MAX_SPEED 2.0   //电机最大速度

ros::NodeHandle *n;

static int controllerCount;
static vector<string> controllerList; 

ros::ServiceClient timeStepClient;          //时钟通讯客户端
webots_ros::set_int timeStepSrv;            //时钟服务数据

/*******************************************************
* 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 :quit
* Description   :退出函数
* Parameter     :
        @sig   信号
* Return        :无
**********************************************************/
void quit(int sig) {
    ROS_INFO("User stopped the '/robot' node.");
    timeStepSrv.request.value = 0; 
    timeStepClient.call(timeStepSrv); 
    ros::shutdown();
    exit(0);
}

int main(int argc, char **argv) {
    setlocale(LC_ALL, ""); // 用于显示中文字符
    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", 100, controllerNameCallback);
    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;
        cout << "Choose the # of the controller you want to use:\n";
        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();

    // main loop
    while (ros::ok()) {
        if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) {
        ROS_ERROR("Failed to call service time_step for next step.");
        break;
        }
        ros::spinOnce();
    }
    timeStepSrv.request.value = 0;
    timeStepClient.call(timeStepSrv);
    ros::shutdown(); 
    return 0;
}
  1. 在上文中,我们知道我们需要三个检测元件,分别是GPS、IMU、激光雷达
  2. 在进行下面的操作前,我们首先要知道各个元件对应的数据类型是什么。
    使用rosservice list查看服务,找到/robot/gps/enable
    在控制台中输入以下命令对其使能:

 

$ rosservice call /robot/gps/enable "value: 32"

success: True

 

使能完后使用rostopic list查看gps是否发布了话题/robot/gps/values
在控制台下输入以下命令获取数据类型:

 

$ rostopic info /robot/gps/values 

Type: sensor_msgs/NavSatFix

Publishers: 
 * /robot (http://mckros-GL553VD:39691/)

Subscribers: None

从上面可以看到数据类型为sensor_msgs/NavSatFix,那我们写程序时头文件就要加入#include <sensor_msgs/NavSatFix.h>
介绍了gps数据类型获取的方法,其他两个元件类似。

 

  1. 需要分别对其使能才能在webots中正常运行。
  • 使能GPS并且获取GPS数据

头文件:

 

#include <sensor_msgs/NavSatFix.h>

订阅gps服务:

 

    ros::ServiceClient gps_Client;          
    webots_ros::set_int gps_Srv;            
    ros::Subscriber gps_sub;
    gps_Client = n->serviceClient<webots_ros::set_int>("/robot/gps/enable"); // 使能GPS服务
    gps_Srv.request.value = TIME_STEP;
    // 判断gps使能服务是否成功
    if (gps_Client.call(gps_Srv) && gps_Srv.response.success) {
        ROS_INFO("gps enabled.");
        // 订阅gps话题,获取数据
        gps_sub = n->subscribe("/robot/gps/values", 1, gpsCallback);
        ROS_INFO("Topic for gps initialized.");
        while (gps_sub.getNumPublishers() == 0) {
        }
        ROS_INFO("Topic for gps connected.");
    } else {
        if (!gps_Srv.response.success)
        ROS_ERROR("Failed to enable gps.");
        return 1;
    }

 

gps回调函数:

 

void gpsCallback(const sensor_msgs::NavSatFix::ConstPtr &values)
{
    GPSvalues[0] = values->latitude;// 纬度
    GPSvalues[1] = values->altitude;// 海拔高度
    GPSvalues[2] = values->longitude;// 经度
    broadcastTransform(); // tf坐标转换
}
  • 使能IMU并且获取IMU数据

 

头文件:

 

#include <sensor_msgs/Imu.h>

订阅IMU服务

 

    ros::ServiceClient inertial_unit_Client;          
    webots_ros::set_int inertial_unit_Srv;            
    ros::Subscriber inertial_unit_sub;
    inertial_unit_Client = n->serviceClient<webots_ros::set_int>("/robot/inertial_unit/enable"); //订阅IMU使能服务
    inertial_unit_Srv.request.value = TIME_STEP;
    // 判断是否使能成功
    if (inertial_unit_Client.call(inertial_unit_Srv) && inertial_unit_Srv.response.success) {
        ROS_INFO("inertial_unit enabled.");
        // 获取话题数据
        inertial_unit_sub = n->subscribe("/robot/inertial_unit/roll_pitch_yaw", 1, inertial_unitCallback);
        ROS_INFO("Topic for inertial_unit initialized.");
        while (inertial_unit_sub.getNumPublishers() == 0) {
        }
        ROS_INFO("Topic for inertial_unit connected.");
    } else {
        if (!inertial_unit_Srv.response.success)
        ROS_ERROR("Failed to enable inertial_unit.");
        return 1;
    }

 

回调函数:

 

void inertial_unitCallback(const sensor_msgs::Imu::ConstPtr &values)
{
    Inertialvalues[0] = values->orientation.x; 
    Inertialvalues[1] = values->orientation.y;
    Inertialvalues[2] = values->orientation.z;
    Inertialvalues[3] = values->orientation.w;
    broadcastTransform(); // tf坐标转换
}
  • 使能激光雷达
      ros::ServiceClient lidar_Client;          
      webots_ros::set_int lidar_Srv;            
      lidar_Client = n->serviceClient<webots_ros::set_int>("/robot/Sick_LMS_291/enable"); // 订阅lidar使能服务
      lidar_Srv.request.value = TIME_STEP;
      // 判断是否使能成功
      if (lidar_Client.call(lidar_Srv) && lidar_Srv.response.success) {
          ROS_INFO("gps enabled.");
      } else {
          if (!lidar_Srv.response.success)
          ROS_ERROR("Failed to enable lidar.");
          return 1;
      }
    
  • tf 坐标转换:
    void broadcastTransform()
    {
      static tf::TransformBroadcaster br;
      tf::Transform transform;
      transform.setOrigin(tf::Vector3(-GPSvalues[2],GPSvalues[0],GPSvalues[1]));// 设置原点
      tf::Quaternion q(Inertialvalues[0],Inertialvalues[1],Inertialvalues[2],Inertialvalues[3]);// 四元数 ->欧拉角
      q = q.inverse();// 反转四元数
      transform.setRotation(q); //设置旋转数据
      br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"odom","base_link"));// 发送tf坐标关系
      transform.setIdentity();
      br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "/robot/Sick_LMS_291"));
    }
    

     

记得将下面这几行加入到CMakelist.txt文件中以正常编译。

 

括号内加入tf
find_package(catkin REQUIRED COMPONENTS )
括号内加入tf
catkin_package()

add_executable(robot_broadcaster src/robot_broadcaster.cpp)
add_dependencies(robot_broadcaster webots_ros_generate_messages_cpp)
target_link_libraries(robot_broadcaster    ${catkin_LIBRARIES})

 

由于要使用到tf包,所以在package.xml中需要添加如下信息:

 

<!--导航-->>
  <build_depend>tf</build_depend>
  <build_export_depend>tf</build_export_depend>
  <exec_depend>tf</exec_depend>
  1. 编译
    $ cd catkin_ws/
    $ catkin_make
    

运行试试

$ rosrun webots_demo robot_broadcaster

运行rqt查看tf tree

$ rosrun rqt_tf_tree rqt_tf_tree

可以看到如下所示图,说明各个节点已经成功连接在一起了。

 

ROS联合webots实战案例(五)导航功能包入门1插图(3)

结语

 

本文也是基于笔者的学习和使用经验总结的,主观性较强,如果有哪些不对的地方或者不明白的地方,欢迎评论区留言交流~

 

Bye

发表评论

后才能评论