Visp_ros学习笔记（二）：在Gazebo环境下实现Pionner3dx移动机器人视觉伺服仿真

开发环境：Unbuntu 18.04 LTS + ROS Melodic + ViSP 3.3.1

本文主要介绍了如何实现Pionner3dx移动机器人视觉伺服仿真，仿真环境是ROS+Gazebo，控制对象是Pioneer3dx（先锋）移动机器人，控制算法借助visp_ros工具实现。视觉伺服控制部分主要参考了visp_ros中的例程tutorial-ros-pioneer-visual-servo.cpp，但由于原本的例程是需要对真实的pionner机器人进行控制，对于没有pionner机器人的同学而言就无法使用了，所以我借助gazebo仿真工具，通过仿真的方法创建一个虚拟的pionner机器人并通过ros实现对机器人的运动控制。整个工程项目我已经上传至github:https://github.com/EmptyCity1995/visual_servo
首先通过一个视频，简单的演示一下视觉伺服控制的效果。

视觉伺服仿真效果演示

可以看到随着目标物体的移动，机器人会自动调整自己的位姿以保证目标始终处于相机的中心。在机器人相机视角观察的效果是这样的，一开始目标物体偏离相机视野中心位置

经过视觉伺服控制机器人运动之后，目标物体回到相机视野中心，绿色线条表示视野中心，红色十字表示目标物体的中心。

下面介绍整个工程实现方法，文件结构如下所示

visual_servo/
├── ar_tags
│ ├── blend
│ ├── images
│ ├── model
│ │ └── marker0
│ │ ├── materials
│ │ │ └── textures
│ │ └── meshes
│ └── scripts
├── include
│ └── visual_servo
├── launch
├── src
├── urdf
└── world

ar_tags是一个用于在gazebo中生成目标模型的脚本工具，原作的项目连接如下https://github.com/mikaelarguedas/gazebo_models；
include中包含了项目需要的头文件；
launch中包含了用于加载gazebo仿真环境的launch文件；
src中包含了视觉伺服程序文件；
urdf中包含了pionner3dx机器人模型文件，原作的项目连接如下https://gitee.com/navigation_planning/pioneer_simulator；
world中包含了gazebo仿真环境文件；

首先需要配置环境，本项目的开发环境为Ubuntu18.04 + ROS Melodic + Visp_ros，所以要把必要的工具包都安装好，关于ViSP，visp_version, visp_ros的安装方法可参见这篇博客：Ubuntu18.04环境下visp、visp_ros、vision_visp手动安装完整指南。
然后就是下载本文的工程文件了，需要将本工程放到ROS的工作空间中

cd ~/catkin_ws/src
git clone git://github.com/EmptyCity1995/visual_servo.git
git clone https://gitee.com/navigation_planning/pioneer_simulator.git
sudo apt install ros-melodic-p2os-urdf
sudo apt install ros-melodic-gazebo-plugins
sudo apt install ros-melodic-gazebo-ros-control

编译一下

cd ~/catkin_ws
catkin_make
source ./devel/setup.bash

利用ar_tags工具创建一个目标模型用于视觉伺服

cd ~/catkin_ws/src/visual_servo/ar_tags/scripts
./generate_markers_model.py -i ../images/

此时在gazebo模型库中会增加一个名为target的模型，路径为~/.gazebo/models/target，生成好模型文件后，我们不着急添加，先把机器人模型加载到仿真环境中

roslaunch visual_servo gazebo_pioneer_amcl.launch

此时程序会自动打开gazebo仿真环境，并添加一个机器人进去

我们可以通过insert添加我们的目标模型

最后则是编译和运行视觉伺服程序

cd ~/catkin_ws/src/visual_servo/src/
mkdir -p build
cd build
cmake ..
make
./tutorial-ros-pioneer-visual-servo

此时屏幕中会出现一个窗口，显示了机器人自带相机所捕捉到的画面

我们用鼠标点击目标模型中的黑色圆圈部分，用于选中视觉伺服的跟踪目标，并开始视觉伺服。小车会自动移动，最终是的目标物体处于相机视野的中央位置

在gazebo环境中移动目标物体，可以发现小车也会跟随目标移动。因为只使用了一个目标点（圆形区域的中心）所以只能对X方向进行约束，视觉伺服效果并不能达到很好，本项目旨在演示视觉伺服的效果和visp_ros+gazebo联合仿真的方法。最后介绍一下视觉伺服控制程序

#include <iostream>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>#include <visp/vpCameraParameters.h>
#include <visp/vpDisplayX.h>
#include <visp/vpDot2.h>
#include <visp/vpFeatureBuilder.h>
#include <visp/vpFeatureDepth.h>
#include <visp/vpFeaturePoint.h>
#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpImage.h>
#include <visp/vpImageConvert.h>
#include <visp/vpServo.h>
#include <visp/vpVelocityTwistMatrix.h>
#include <visual_servo/vpSimulatorPioneer.h>//这个头文件中包含了pioneer机器人仿真参数及相关函数的声明
#include <visp_ros/vpROSGrabber.h>
// #include <visp_ros/vpROSRobotPioneer.h>#if defined(VISP_HAVE_DC1394_2) && defined(VISP_HAVE_X11)
#  define TEST_COULD_BE_ACHIEVED
#endif#ifdef TEST_COULD_BE_ACHIEVEDint main(int argc, char **argv)
{ros::init(argc, argv, "velocity_publisher");//初始化速度话题发布// 创建节点句柄ros::NodeHandle n;// 创建一个Publisher，发布名为/pionner/cmd_vel的topic，消息类型为geometry_msgs::Twist，队列长度10ros::Publisher pionner_vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 10);geometry_msgs::Twist vel_msg;//创建一个geometry_msgs::Twist类型的速度消息try {vpImage<unsigned char> I; // Create a gray level image containerdouble depth = 1.; //创建一个期望的距离（目标与小车之间）double lambda = 0.6; //视觉伺服增益参数double coef = 0.25;// 用于计算目标和小车之间距离的比例参数vpSimulatorPioneer robot; //创建一个pionner的仿真实例vpCameraParameters cam; //创建相机参数实例vpROSGrabber g;//创建图像捕捉实例g.setCameraInfoTopic("/camera_rgb/camera_info");//设置相机信息话题g.setImageTopic("/camera_rgb/image_raw");//设置图像话题g.setRectify(true);// Set camera parameterscam.initPersProjWithoutDistortion(600,600,I.getWidth()/2, I.getHeight()/2);//设置相机参数g.open(I);//打开图像捕捉实例g.acquire(I);//获取第一帧图像//创建图像显示器，并显示图像vpDisplayX d(I, 10, 10, "Current frame");vpDisplay::display(I);vpDisplay::flush(I);// 创建一个连通区域跟踪器，根据鼠标点击来选择连通区域vpDot2 dot;dot.setGraphics(true);dot.setComputeMoments(true);dot.setEllipsoidShapePrecision(0.);  // to track a blob without any constraint on the shapedot.setGrayLevelPrecision(0.9);  // to set the blob gray level bounds for binarisationdot.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray leveldot.initTracking(I);//跟踪连通区域//输出连通区域中心的坐标std::cout << "dot_cog_x: " <<  dot.getCog().get_u()<< "dot_cog_y: " <<  dot.getCog().get_v()<< std::endl;std::cout <<"dot_size:"<<dot.m00<<std::endl ;//输出连通区域的一阶图像距，可以表示图像的面积信息vpDisplay::flush(I);//创建一个视觉伺服人物vpServo task;task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;//设置视觉伺服计算方法eyeinhand cVe_eJetask.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;//设置交互矩阵类型task.setLambda(lambda) ;//设置增益参数vpVelocityTwistMatrix cVe ;//创建cVe矩阵，描述相机与机器人之间的坐标变换关系cVe = robot.get_cVe() ;//从机器人中获取cVe矩阵信息task.set_cVe(cVe) ;//将cVe矩阵添加到视觉伺服任务中std::cout << "cVe: \n" << cVe << std::endl;//打印cVe矩阵参数vpMatrix eJe;//创建eJe矩阵，机器人雅可比矩阵robot.get_eJe(eJe) ;//从机器人中获取eJe矩阵信息task.set_eJe(eJe) ;//将eJe矩阵添加到视觉伺服任务中std::cout << "eJe: \n" << eJe << std::endl;//打印eJe矩阵参数// 创建目标中心点的x坐标特征，s_x表示当前位置，s_xd表示期望位置vpFeaturePoint s_x, s_xd;// 根据相机参数和连通区域中心点的坐标获取当前位置下的x坐标特征vpFeatureBuilder::create(s_x, cam, dot);// 创建期望位置处的x坐标特征，0.5表示x方向是图像中心，0.89是y方向对应的图像位置，depth是空间坐标系下的距离s_xd.buildFrom(0.5, 0.89, depth);// 把x坐标特征添加到视觉伺服任务中task.addFeature(s_x, s_xd) ;// 创建目标中心点的z坐标特征log(Z/Zd)，s_Z表示当前位置，s_Zd表示期望位置vpFeatureDepth s_Z, s_Zd;//根据连通区域的面积来估计相机与目标之间的距离，距离的平方与面积成反比例关系，面积约小表示距离越远（近大远小）double surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));double Z, Zd;//创建目标中心点的空间距离参数，Z表示当前位置，Zd表示期望位置Z = coef * surface ;      //初始化目标与相机之间的距离Zd = Z;// 设置期望距离等于当前距离std::cout << "Z :" << Z << std::endl; //输出当前位置处的距离信息s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0); // 设置当前位置处的Z坐标特征，因为Z=Zd，所以特征log(Z/Zd)=0s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd , 0); // 设置期望位置处的Z坐标特征，因为Z=Zd，所以特征log(Z/Zd)=0task.addFeature(s_Z, s_Zd) ;//把Z坐标特征添加到视觉伺服任务中vpColVector v; // 创建速度向量while(1){g.acquire(I);//获取相机图像vpDisplay::display(I);//显示相机图像内容dot.track(I);//跟踪目标连通区域vpFeatureBuilder::create(s_x, cam, dot);//更新x坐标特征// 更新Z坐标特征surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));Z = coef * surface ;s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;//更新cVe矩阵信息robot.get_cVe(cVe) ;task.set_cVe(cVe) ;//更新eJe矩阵信息robot.get_eJe(eJe) ;task.set_eJe(eJe) ;// 根据视觉伺服算法计算控制律v = task.computeControlLaw() ;//输出速度信息std::cout << "Send velocity to the pionner: " << v[0] << " m/s "<< vpMath::deg(v[1]) << " deg/s" << std::endl;// 将速度指令发送到机器人vel_msg.linear.x =v[0];vel_msg.angular.z =vpMath::deg(v[1]);// 发布消息pionner_vel_pub.publish(vel_msg);ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z);std::cout <<"s_x:"<<s_x.get_x()<<std::endl;std::cout <<"s_y:"<<s_x.get_y()<<std::endl ;std::cout << "dot_cog_x: " <<  dot.getCog().get_u()<< "dot_cog_y: " <<  dot.getCog().get_v()<< std::endl;std::cout <<"Z:"<<Z<<std::endl ;std::cout <<"Error:"<<task.getError()[0]<<std::endl;//输出x方向上的误差信息// 绘制一条绿色的竖线表示x方向上的期望位置vpDisplay::displayLine(I, 0, 300, 600, 300, vpColor::green);vpDisplay::flush(I);// 点击图像退出if ( vpDisplay::getClick(I, false) )break;//当x方向的误差小于阈值时，命令机器人停止运动if (abs(task.getError()[0])< 0.001) {std::cout << "Reached a small error. We stop the loop... " << std::endl;std::cout << "Ending robot thread..." << std::endl;vel_msg.linear.x =0;vel_msg.angular.z = 0;pionner_vel_pub.publish(vel_msg);std::cout << "Robot  is stopped" << std::endl;}};//程序结束后，命令小车停止运动std::cout << "Ending robot thread..." << std::endl;vel_msg.linear.x =0;vel_msg.angular.z = 0;pionner_vel_pub.publish(vel_msg);std::cout << "Robot  is stopped" << std::endl;task.print() ;//输出视觉伺服控制信息task.kill();//结束视觉伺服控制}catch(vpException e) {std::cout << "Catch an exception: " << e << std::endl;//有异常抛出时，命令机器人停止运动vel_msg.linear.x =0;vel_msg.angular.z = 0；pionner_vel_pub.publish(vel_msg);std::cout << "Robot  is stopped" << std::endl;return 1;}
}
#else
int main()
{std::cout << "You don't have the right 3rd party libraries to run this example..." << std::endl;
}
#endif

如果大家对于深度学习与计算机视觉领域感兴趣，希望获得更多的知识分享与最新的论文解读，欢迎关注我的个人公众号“深视”。

Visp_ros学习笔记（二）：在Gazebo环境下实现Pionner3dx移动机器人视觉伺服仿真相关推荐

ViSP学习笔记（四）：在Gazebo环境下实现Pionner3dx移动机器人基于四个特征点的视觉伺服仿真
开发环境:Unbuntu 18.04 LTS + ROS Melodic + ViSP 3.3.1 文章内容主要参考ViSP官方教学文档:https://visp-doc.inria.fr/doxyg ...
Redis学习笔记~Redis在windows环境下的安装
Redis是一个key-value的存储系统,它最大的特点就是可以将数据序列化到文件中. redis存储在服务器的内存或者文件中,它不是session,不是cookies,它只是个更安全,更稳定,更可 ...
《ESP32 学习笔记》之Arduino环境下使用DAC模拟输出（是真的DAC哦！）完成两路呼吸灯
在 Arduino 环境下经常使用的 analogWrite(PIN, arg) 语法并不是真正的DAC模拟输出,也只是1kHZ的PWM 波模拟而成! 支持DAC功能的引脚请查看:引脚定义本次ESP ...
RabbitMQ学习系列二：.net 环境下 C#代码使用 RabbitMQ 消息队列
上一篇已经讲了Rabbitmq如何在Windows平台安装,不懂请移步:RabbitMQ学习系列一:windows下安装RabbitMQ服务一.理论: .net环境下,C#代码调用RabbitMQ消 ...
大数据学习笔记二：Ubuntu/Debian 下安装大数据框架Hadoop
文章目录安装Java 为Hadoop创建用户安装Hadoop 配置Hadoop 配置环境变量设置配置文件格式化namenode 启动hadoop集群访问hadoop集群大数据学习系列文章: ...
SEO学习笔记二：在搜索引擎竞价排名环境下，个人网站将何去何从?
本文首发于「妙蛙种子前端」博客,欢迎关注- 早期的搜索引擎,大家都在一个相对公平的规则内玩耍:你的内容够好,网站体验更优秀,在搜索引擎中的排名一般都会比较高. 因为搜索引擎能便捷的为我们定位到精准的内 ...
④ESP8266 开发学习笔记_By_GYC 【Ubuntu系统下ESP8266 开发环境搭建】
目录 ④ESP8266 开发学习笔记_By_GYC [Ubuntu系统下ESP8266 开发环境搭建] 一.安装前准备 1.乐鑫官方的ESP-IDF 编程指南 2.ESP-IDF风格的ESP8266 ...
Spring Boot 框架学习笔记(二）（配置文件与数据注入 yaml基本语法 JSR303数据验证多环境切换）
Spring Boot 框架学习笔记(二) 六.appliaction.properties配置与数据注入 6.1 `@Value`注解测试注入数据读取输入流 6.2 读取配置文件数据注入单文件 ...
Kotlin学习笔记（1）- 环境配置
系列文章全部为本人的学习笔记,若有任何不妥之处,随时欢迎拍砖指正.如果你觉得我的文章对你有用,欢迎关注我,我们一起学习进步!kotlin学习笔记系列首发简书和CSDN Kotlin学习笔记(1)- 环 ...

Visp_ros学习笔记（二）：在Gazebo环境下实现Pionner3dx移动机器人视觉伺服仿真

Visp_ros学习笔记（二）：在Gazebo环境下实现Pionner3dx移动机器人视觉伺服仿真相关推荐

最新文章

热门文章