序
简介
HIROP是一款基于ROS的智能机器人开放开发平台,主要目的在于为智能机器人开放者提供更为完善的软件架构及相关功能软件包。
在ROS的基础上补充了相关感知、识别、抓取等功能模块。
支持ROS的版本
indigo
kinetic
版本
V1.0
V0.1
入门
环境搭建
当前教程基于ROS indigo
ROS环境搭建
设置源
sudo sh -c '. /etc/lsb-release && echo "deb http://mirrors.ustc.edu.cn/ros/ubuntu/ $DISTRIB_CODENAME main" > /etc/apt/sources.list.d/ros-latest.list'
设置密钥
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
更新源
sudo apt-get update
sudo apt-get upgrade
安装ROS
sudo apt-get install ros-indigo-desktop-full
初始化rosdep
sudo rosdep init
更新rosdep
rosdep update
设置环境变量
echo source /opt/ros/indigo/setup.bash >> ~/.bashrc
HIROP环境搭建
创建工作区
$ cd ~; mkdir catkin_ws
$ cd catkin_ws; mkdir src
$ catkin_make
$ echo source ~/catkin_ws/devel/setup.bash >> ~/.bashrc
下载源码
sudo apt-get install git
cd ~/catkin_ws/src
git clone https://github.com/HSRobot/hsr_irp.git
下载依赖项
sudo apt-get install ros-indigo-industrial-core
sudo apt-get install ros-indigo-moveit-full
sudo apt-get install ros-indigo-serial
sudo apt-get install ros-indigo-gazebo-ros-control
sudo apt-get install ros-indigo-joint-trajectory-controller
sudo apt-get install ros-indigo-joint-state-controller
sudo apt-get install ros-indigo-effort-controllers
sudo apt-get install ros-indigo-position-controllers
编译
catkin_make
快速开始
1.简介
此仓库为HIROP的软件平台,包含华数机器人部分机械臂的URDF模型、moveit配置、gazebo配置、ROS-I驱动包、夹爪驱动包、rviz控制面板、抓取Demo、抓取仿真环境、基础Demo等示例开发包。
此平台遵循BSD开源协议。
支持indigo、kinetic版本。如需下载kinetic版本,请将分支切换至kinetic分支。
维护者: Kunlin Xu(1125290220@qq.com)
2.准备工作
2.1 环境安装
请先对ROS环境进行安装,支持indigo以及kinetic版本。
ROS相关教程可参考:完整的安装教程
2.2安装gazebo模型
由于indigo版本的gazebo模型源地址更换,所以无法自动获取完整的gazebo模型,按照以下步骤进行离线安装:
1.从此链接处下载完整的gazebo模型压缩包
2.将模型解压至 ~/.gazebo/modles 文件夹中
3.实验之前说明
HIROP平台支持多型号机械臂,因此以下所有实验均使用br606机械臂作为示例说明。若用户想进行其它支持的机械臂的仿真,则只需将指令中的br606替换成对应型号即可。
4.HIROP的基础使用教程
4.1 加载机械臂模型
roslaunch hsr_description br606_upload.launch
4.2 在rviz中显示模型
$ roslaunch hsr_description display.launch
4.3 启动仿真环境
4.3.1 一键启动:
roslaunch hsr_bringup br606_sim.launch
4.3.2 单独启动(推荐):
roslaunch hsr_description br606_upload.launch
roslaunch hsr_gazebo br606_gazebo.launch
roslaunch br606_moveit_config hsr_br606_moveit_planning_execution.launch sim:=true
roslaunch br606_moveit_config moveit_rviz.launch
4.4 使用gazebo中的相机
roslaunch hsr_description br606_with_kinect_upload.launch
roslaunch hsr_gazebo robot_gazebo.launch
4.5 虚拟抓取
roslaunch hsr_description br606_with_gripper_upload.launch
roslaunch br606_moveit_config hsr_br606_moveit_planning_execution.launch sim:=true
roslaunch br606_moveit_config moveit_rviz.launch
rosrun hsr_demo moveit_pick_and_place_demo.py
4.6 连接实际的机器人
4.6.1 一键启动:
roslaunch hsr_bringup br606_go.launch robot_ip:=10.10.56.214
4.6.2 单独启动(推荐):
roslaunch hsr_description br606_upload.launch
roslaunch br606_moveit_config br606_moveit_planning_execution.launch sim:=false
roslaunch hsr_rois_client robot_full_interface_download.launch robot_ip:=10.10.56.214
roslaunch br606_moveit_config moveit_rviz.launch
4.7 启动pick(已知物体位姿)
roslaunch hsr_description br606_with_gripper_upload.launch
roslaunch br606_moveit_config br606_moveit_planning_execution.launch sim:=false
roslaunch hsr_rosi_client robot_full_interface_download.launch robot_ip:=10.10.56.214
rosrun hsr_gripper_driver gripper_control_srv
rosrun hsr_gripper_driver gripper_action.py
rosrun hsr_pick moveit_pick_and_place_demo.py
4.8 启动pick and ork(前提需要对相机进行标定)
roslaunch kinect2_bridge kinect2_bridge.launch
roslaunch hsr_description br606_with_gripper_upload.launch
roslaunch br606_moveit_config br606_moveit_planning_execution.launch sim:=false
roslaunch hsr_rosi_client robot_full_interface_download.launch robot_ip:=10.10.56.214
rosrun hsr_gripper_driver gripper_control_srv
rosrun hsr_gripper_driver gripper_action.py
roslaunch hsr_bringup ork_base_tf.launch
rosrun hsr_pick tf_camera_to_base.py
rosrun object_recognition_core detection -c `rospack find object_recognition_linemod`/conf/detection.ros.ork
rosrun hsr_pick moveit_pick_and_place_demo.py
4.9 启动pick and ork (RVIZ GUI)
roslaunch hsr_bringup pick_gui.launch
参考手册
参考手册主要提供HIROP
平台的指令、包、launch、错误介绍等相关使用指南。
手册列表
agv使用手册
CO602A AGV使用教程
通过本教程可以习得华数CO602A+AGV复合机器人的使用。
环境配置
在开始前需要做以下的初始配置。
连接wifi
复合机器人装有wifi发射器。
wifi名称:WIFI-03D148
wifi密码:无密码
登录复合机器人控制器
通过ssh可以登录到复合机器人的控制器中
用户名称:shansu
密码:123
$ ssh shansu@192.168.99.30
$ # 输入密码123
Note
本文后面中提到的登录复合机器人控制器,均可参考此处
分布式配置
在本平台中,是通过wifi与复合机器人的控制器进行通讯,在复合机器人控制器运行ROS的master节点。用户上位机需要与复合机器人底盘通讯则需进行分布式配置。步骤如下: 1,获取本机IP地址和主机名称
$ ifconfig #获取本机IP地址
$ cat /etc/hostname #获取本机的主机名称
2,将本机IP地址和主机名称写入复合机器人控制器的hosts文件中
$ ssh shansu@192.168.99.30 #登录底盘
$ vim /etc/hosts #编辑hosts文件
在最后一行中加入:
本机的IP地址 本机的主机名称
例如:
192.168.99.233 fshs-kinetic
3,将复合机器人控制器的IP地址和主机名称写入本机的hosts文件中
$ vim /etc/hosts #编辑本机的hosts文件
在最后一行中加入:
192.168.99.30 shansurobotics
4,配置环境变量 可以通过指令的形式来导入环境变量
$ export ROS_HOSTNAME=本机主机的名称 # 如:export ROS_HOSTNAME=fshs-kinetic
$ export ROS_MASTER_URI=http://shansurobotics:11311
或者将其写入至脚步文件中,然后通过source指令来导入环境变量,方便使用。
Note
由于配置分布式后,讲无法与本机的ROS程序通讯,因此不建议将环境变量直接写入至.bashrc文件中
基础教程
在完成上述的基础环境配置后,就可以开始我们的基础使用教程了。
建图
不管在进行底盘导航还是移动物体抓取时,首先都需要将环境的地图构建出来。
$ ssh shansu@192.168.99.30 #登录底盘
$ roslaunch ir100_navigation ir100_gmapping.launch
在建图过程中,可以在本机上可视化建图过程
$ roslaunch hsr_bringup view_nav.launch #该指令在本机上执行
执行完上述指令后,移动底盘便已经开始建图了,这时可以通过操控手柄来控制底盘走一圈需要被构建的环境。
Note
在构建地图的时候,人应当站在移动底盘的后方来控制移动底盘运动。且尽量避免弧形的运动,需要转向时最好先讲移动底盘停止下来在进行旋转
当已经控制移动底盘绕环境走一圈后就可以保存地图了。这时先不要关闭建图程序
,新开一个终端执行以下指令
$ ssh shansu@192.168.99.30 #登录底盘
$ rosrun map_server map_saver -f ~/work/maps/test_map #保存地图至~/work/maps/目录,地图名称为test_map
Note
当保存完地图后再讲建图程序关闭
导航
在将地图构建完成后便可进行导航实验。
1,在复合机器人控制器上启动导航程序
$ ssh shansu@192.168.99.30 #登录底盘
$ roslaunch ir100_navigation ir100_navigation.launch
2,在本机上可视化导航过程
$ roslaunch hsr_bringup view_nav.launch
3,给定机器人的初始位置 点击2D Pose Estimate工具,在地图上标记当前实际机器人在地图上的位置
4,更新机器人定位 在给定机器人的初始位置后,可能会有偏差,可以通过下面的指令来更新迭代机器人的定位
$ rosservice call /request_nomotion_update
Note
可以多次执行直到机器人定位较为精确后。
5,给定目标点 在给定和更新机器人的初始位置后,便可以为机器人指定目标点,来让机器人导航至目标点。 点击RVIZ中的2D Nav Goal工具,在地图上标记一个目标点。标记后,机器人便会开始运动。
创建地图标记
为了进行移动抓取应用,我们需要在地图上创建标记来协助进行移动抓取。 在导航程序没有关闭的情况下,使用RVIZ中的map flag工具来对地图进行标记。
1,点击RVIZ菜单栏中的Panels -> Add New Panels,选择rviz_map_tool下的FlagManager;
2,点击工具栏中的+,选择添加rviz_map_tool下的MapFlag;
3,选择工具栏中的Map Flag工具,然后再地图上标记第一个标记点。(对应物理世界的桌子处)
4,在FlagManager窗口中,Map名称设置为demo,Flag名称设置为table,并点击Add Flag按钮
5,按照第3、4步,添加多一个标记点(对应物理世界中的任意一个空旷处),并将其Flag名称设置为home,并添加。
在创建完flag后,需要将flag信息下载到复合机器人控制器中,指令如下
$ scp -r ~/.hirop/data/MAP/demo/ shansu@192.168.99.30:~/.hirop/data/MAP/demo
测试移动抓取应用
在掌握上述的基础教程后,就可以开始学习如何使用移动抓取应用了。
应用介绍
该应用启动后,复合机器人会自动导航至桌子前方。然后使用手眼相机识别桌面上的牛奶盒,并且将牛奶盒抓取放置在移动地盘上。 当桌面上的所有牛奶盒都被抓取放置后,复合机器人便会导航回到home点。
启动环境
1,启动依赖环境
$ ssh shansu@192.168.99.30 #登录复合机器人控制器
$ roslaunch hsr_agv_bringup move_pick_app_go.launch
2,启动感知模块 新建一个终端
$ ssh shansu@192.168.99.30 #登录复合机器人控制器
$ rosrun perception_bridge perception_bridge.py
3,初始化 新建一个终端
$ rosservice call /set_robot_enable true #给机械臂上使能
$ rosservice call /serial_open /dev/ttyUSB0 115200 #设置夹爪的串口号及波特率
$ rosservice call /set_pick_place_vel_acc 0.3 0.05 #设置抓取时机械臂的速度和加速度
4, 定位 参考导航小节中为机器人指定初始位姿的来指定当前机器人在实际环境中的位置。
5,启动应用 新建一个终端
$ ssh shansu@192.168.99.30 #登录复合机器人控制器
$ rosrun app_move_pick app_move_pick
$ # 键入7,执行连续抓取
参数修改
在使用过程中,如果是严格按照上述教程进行实验则无须修改相关参数。但如果修改了相关名称或者有特殊需求,则需要修改相关参数来进行适配。
Note
学习修改相关的参数也有助与理解应用的原理
1,修改地图名称:查看和修改hsr_agv_bringup move_pick_app_go.launch
2,修改标记地图名称:修改app_move_pick中的auto_move_pick.cpp的第20行
3,修改物体放置的位姿:修改app_move_pick中的auto_move_pick.cpp中的initPlace函数。
开发指南
开发指南主要包含HIROP
平台模块开发编程指南及ROS
相关组件的编程指南。
指南列表
HPlugin插件开发指南
HPlugin是一个轻量级的插件模块,可依赖HPlugin框架迅速为工程实现插件化。
编译安装
$ mkdir build; cd build
$ qmake ./
$ make ; make install
制作插件
声明插件接口
在接口的定义头文件中使用H_DECLARE_INTERFACE宏来声明一个接口。
class ISimaplePlugin{
public:
virtual int SayHi(std::string name) = 0;
};
H_DECLARE_INTERFACE(ISimaplePlugin,"ISimaplePlugin/v1.0")
Note
如果接口处于某个命名空间中,那么该宏应当在命名空间外部使用。 H_DECLARE_INTERFACE(namespace::ISimaplePlugin,"ISimaplePlugin/v1.0")
声明插件
插件都是继承于接口的,假如我们有插件A,其类名为SimaplePluginA
,其继承于ISimaplePlugin
。 那么在定义插件类的头文件中,我们需要使用H_DECLARE_PLUGIN
宏来声明一个插件,其中宏的参数是表明该插件式基于哪个接口的。
class SimaplePluginA:public ISimaplePlugin{
public:
SimaplePluginA();
int SayHi(std::string name);
};
H_DECLARE_PLUGIN(ISimaplePlugin)
导出插件
经过上面的两步,定义接口和定义插件。那么接下来,我们就需要给我们定义的插件进行实现和导出,只有导出后的插件,才能被插件系统所加载。 因此,在插件的实现文件中,我们应当使用H_EXPORT_PLUGIN
宏来导出一个插件。
SimaplePluginA::SimaplePluginA(){
std::cout << "SimaplePluginA init" << std::endl;
}
int SimaplePluginA::SayHi(std::string name){
std::cout << "SimaplePluginA say = " << name << " lol" << std::endl;
return 0;
}
H_EXPORT_PLUGIN(SimaplePluginA, "SimaplePluginA", "1.0")
其中宏H_EXPORT_PLUGIN
有三个参数 * 声明要导出的插件类 * 声明导出插件的名称 '需要和上一个参数一致' * 声明导出插件的版本
Note
在工程配置中,应当注意生成的插件动态库的名称为"lib"+插件类名+".so"。如:插件名为SimaplePluginA
,则其动态库名称为libSimaplePluginA.so
调用插件
插件加载器
为了加载插件,我们需要在我们的使用插件系统的程序上,获取我们的插件加载器。
int main(){
HPluginLoader *loader
loader = HPluginLoader::getLoader();
}
通过HPluginLoader::getLoader()函数,来获取当前系统的插件加载器指针。
在加载完插件加载器后,我们需要为插件加载器指定,插件的加载路径,而这一动作我们通过
loader->setPath("./");
这一函数来指定,如果传递的路径为空,那么插件加载器便会通过LD_LIBRARY_PATH环境变量来搜索相关的库。
加载插件
获取完插件加载器和设置相关路径后,便可以通过插件加载器来加载相关的插件了。我们通过一个Hplugin来描述一个插件。
HPlugin *p;
p = loader->load("SimaplePluginA");
if(!p){
cout << "load plugin error" << endl;
return -1;
}
当我们通过load函数加载插件失败时,它将会返回一个空指针。开发者很有必要对该值进行判断。
实例化插件
在我们通过上面几步后,我们拿到了一个插件对象Hplgun,但是这个插件对象中并不是我们真正的插件类的指针。 我们还需要将其实例化后,才能拿到插件的指针。
ISimaplePlugin *pluginPtr;
pluginPtr = p->instance<ISimaplePlugin>();
if(!pluginPtr){
cout << "instance plugin error" << endl;
return -1;
}
我们将通过instance
函数来获取一个插件的实例指针,它是一个模板函数,需要指明要实例化的插件接口类。通过模板函数来防止开发者将错误的实例赋值给插件。 如果通过instance获取实例化插件失败(类型错误或其他原因)时,将会返回一个空指针。
我们通过instance
获取到插件实例后,便可以使用该实例来使用插件的功能了。
pluginPtr->SayHi("FSHS");
当使用完插件后,我们需要将插件进行卸载。
loader->unload(p);
Vision模块编程指南
引言
通过该学习该指南可以了解开发HIROP平台vision模块的基础流程
准备
在开始前,我们需要熟悉和了解以下概念。
接口
在HIROP vision模块中,将开发接口分为了用户接口和开发者接口。
用户接口
用户接口是提供给想使用VISION识别功能开发机器人应用程序的开发者使用的。
开发者接口
开发者接口则是为那些想将自己或其他视觉算法集成至vision中算法开发者和集成者使用的。 而本文主要就是描述如何将一个视觉算法集成至vision中。
模块
为了集成视觉算法至VISION模块中,我们还需要了解以下概念:训练器和识别器。在集成相关算法时,实际上就是编写这两个模块。
训练器
由于VISION模块可以适配使用机器学习及其他需要进行训练的视觉算法,因此VISION模块中独立分出来了训练器的概念:用户算法训练功能实现的模块。 如果使用的算法不需要进行训练,那么则无需为算法编写训练器。
识别器
识别器就是用户算法进行实际物体识别的模块,所有的用户算法都必须实现改模块。
开发语言
Vision模块支持使用Python和C++两种语言来编写相关算法的实例。不管使用何种语言其开发者接口都是一致的。
开始
通过了解了上述概念后我们就开始进入我们的开发中了。现在假设我们有一物体识别算法A,其识别之前需要进行训练。因此我们需要开发两个模块:A训练器和A识别器。 下面我将分别讲解C++和Python如何编写算法实例。
C++
编写训练器
// 开发者接口源文件
#include <vision/c_base_trainer.h>
class ATrainer : public CBaseTrainer{
public:
ATrainer();
/**
* @brief A训练器的具体训练实现函数
* @return 0 训练成功 -1 训练失败
*/
int train(){
/**
* 具体训练的实现
*/
}
/**
* @brief 解析由创建传递过来的算法的配置参数
* @param
* @return 0 成功 -1 失败
*/
int parseConfig(const YAML::Node &node){
/**
* 解析参数,比如获取训练的次数,数据集的的路径等
*/
}
/**
* @brief 获取训练进度
* @param
* @return 0 > 获取失败 >=0 进度
*/
int feedback(){
/**
* 获取并返回当前的训练进度
*/
}
/**
* @brief 保存训练结果
* @param 保存路径
* @return 0 成功 -1 失败
*/
int saveData(std::string path){
/**
* 保存相关训练的结果,保存在path路径下
*/
}
}
// 定义该类为一个插件,继承自训练器
H_DECLARE_PLUGIN(ITrainer)
H_EXPORT_PLUGIN(ATrainer, "ATrainer", "1.0")
Note
在编写算法实例时,我们的训练器实例需要继承与ITrainer
接口。但是在这里我们却继承了CBaseTrainer
?这是为什么呢? 主要是因为Vision模块中为训练器提供了辅助类,帮助我们实现了部分简单的ITrainer
接口,降低开发者的开发难度。
在ATrainer中,我需要实现:
int train()
:该函数就是主要进行物体训练的接口了,算法的具体训练功能在该接口中(阻塞)实现。int parseConfig()
:对于需要一些参数的训练器而言,该函数主要是用于从Vision模块中传递相关的训练器参数的,传递是Yaml对象。对于无需训练参数的算法实例,则无需实现。int feedback()
:该函数主要的目的在于向Vision模块反馈当前训练的进度,如果想为你的训练器提供进度获取的功能,则可在该函数中具体实现获取进度并返回,如不提供进度功能,则无需实现。int saveData()
:由于当期VISION模块中不提供数据保存相关的接口,这就意味着开发者需要自己编写代码来保存相关的训练数据。不过Vison为了方便管理训练数据,会为每个训练器提供一个专有的目录。因此,训练器需要将所有的训练数据保存至指定路径path中。
实现上面的四个接口后,我们的Vision便可以识别和使用ATrainer了。不过在最后,我们需要声明,我们这个类是一个插件。因为在Vision中,所有的算法实例都是以插件的形式存在
H_DECLARE_PLUGIN(ITrainer)
H_EXPORT_PLUGIN(ATrainer, "ATrainer", "1.0")
对于该宏的使用和定义可参考HPluin开发指南。
训练器接口调用顺序
对于上述ATrainer实现的接口,Vision模块对训练接口的调用时机如下。
- Vision模块首先根据用户请求,调用
ATrainer
构造函数 - 然后调用
parseConfig()
向ATrainer传递训练的信息,如:物体名称、物体模型路径、数据集路径等 - 在通过parseConfig解析完相关参数后,vision模块就会调用
train()
接口来进行训练 - 当通过train()训练完毕后,Vision模块便会调用
saveData()
接口来通知ATrainer进行数据的保存
编写识别器
#include <vision/c_base_detector.h>
class ADetector : public CBaseDetector{
/**
* @brief 实现具体的识别功能
* @return 0 成功 -1 失败
*/
int detection(){
/**
* 编写具体的识别的代码
*/
}
/**
* @brief 加载相关的训练结果
* @param [objectName] 需要识别的物体
* @return 0 成功 -1 失败
*/
int loadData(const std::string path, const std::string objectName){
/**
* 编写从path路径下加载相关的训练文件,如果有的话。具体如何加载是和相关训练器约定好的
*/
}
/**
* @brief 获取识别的结果
* @param[out] poses, 保存识别的结果
* @return 0 成功 -1 失败
*/
int getResult(std::vector<pose> &poses){
/**
* 获取具体的识别结果,返回的是位姿
*/
}
/**
* @brief 向检测器传递图像数据
* @param [inputImg] 输入,传递的图像
* @return void
*/
void setDepthImg(const cv::Mat &inputImg){
/**
* 编写保存相关深度图的代码
*/
}
/**
* @brief 向检测器传递图像数据
* @param [inputImg] 输入,传递的图像
* @return void
*/
void setColorImg(const cv::Mat &inputImg){
/**
* 编写保存相关彩色图的代码
*/
}
H_DECLARE_PLUGIN(IDetector)
H_EXPORT_PLUGIN(ADetector, "ADetector", "1.0")
}
在编写相关算法实例的识别器的时候,我们的识别器实例需要继承IDetector
,而此处继承了CBaseDetector,具体原因也是和训练器说的一致。
在ADetector中,我们需要实现以下接口
int detection()
:该接口主要就是实现具体的物体识别逻辑,该接口会由Vision模块调用int loadData()
:该接口的主要目的就是为了加载被识别物体的训练参数。而相关参数就是保存在path路径下的。而具体的数据的保存格式则是ATrainer定义的。int getResult()
:该接口的主要作用就是当识别结束后,Vision调用该接口来获取具体的识别结果。应次在该结果中实现识别结果的返回。void setDepthImg()
:该接口的主要作用就是保存由Vision模块传递给识别器的深度图像数据,供识别的时候使用,如果识别器识别不需要深度图,则该接口无需实现。void setColorImg()
:该接口的主要作用就是保存由Vision模块传递给识别器的彩色图像数据,供识别的时候使用,如果识别器识别不需要彩色图,则该接口无需实现。
同样的,在实现完上诉接口后,我们需要导出我们的识别器
H_DECLARE_PLUGIN(IDetector)
H_EXPORT_PLUGIN(ADetector, "ADetector", "1.0")
对于该宏的使用和定义可参考HPluin开发指南。
训练器接口调用顺序
对于上述ADetector实现的接口,Vision模块对其接口的调用时机如下。
- Vision模块首先根据用户请求,调用
ADetector()
构造函数 - 然后调用ADetector中的
loadData()
接口,来通知ADetector来加载被识别物体的训练参数 - 在加载完相关参数后,Vision模块便会通过
setDepthImg()
和setColorImg()
接口向ADetector传递待识别的图像数据 - 在传递完相关参数后,Vision模块便会调用ADetector的
detection()
接口来进行物体识别 - 当通过detection()接口执行完成,即识别完成后,Vision模块就会调用
getResult()
接口来获取识别的结果
编译
使用下面的Cmake文件来进行工程配置
cmake_minimum_required (VERSION 2.6)
project (VisionDemo)
add_library(ADetector ADetector.cpp)
add_library(ATrainer ATrainer.cpp)
find_package(hirop_vision REQUIRED)
INCLUDE_DIRECTORIES(${hirop_vision_INCLUDE_DIRS})
TARGET_LINK_LIBRARIES(ADetector ${hirop_vision_LIBRARIES})
TARGET_LINK_LIBRARIES(ATrainer ${hirop_vision_LIBRARIES})
$ cmake ./
$ make
$ export VISION_PLUGIN_PATH=$VISION_PLUGIN_PATH:\`pwd\`
测试
可使用以下指令进行测试
$ rosrun vision_bridge vision_bridge
$ rosservice call /detection cokecan A 0 ""
其中第二条指令$ rosservice call /detection cokecan A 0 ""
cokecan
: 待识别物体名称A
: 识别器名称0
: 识别器的类别, 0为C++ 1为python
Python
对于Python开发者,所有的接口的和C++版本均一致,只是将编程语言变成了Python。下面我们只以编写识别器来作为例子讲解即可。
编写识别器
class ADetector:
def __init__(self):
# do some init
def detection(self):
# do something detection
def loadData(self, path, objectname):
# load data from path
def getResult(self, poses):
# returen result
def setDepthImg(self, img):
# save depth image
def setColorImg(self, img):
# save color image
实现一个Python版本的识别器只需实现一个拥有上述接口的类即可。其作用和调用顺序和C++版本的一致
编译
无需编译,只需将python源文件命名为ADetector.py并放置在VISION_PLUGIN_PATH环境变量路径下即可
测试
$ rosrun vision_bridge vision_bridge
$ rosservice call /detection cokecan A 1 ""
感知模块编程指南
HIROP感知模块编程指南
通过学习本指南可以掌握如何开发一个HIROP的感知模块插件。
感知模块介绍
hirop perception模块可以使我们的机器人具有感知能力。
在HIROP中,将感知分为两部分:前端和后端。 其中,前端指通过各种传感器(如:视觉、触觉、听觉)及相关处理算法(如:计算机视觉、PCL)以尽量精确的方式进行环境建模。 后端指的是,当通过前端将环境建模后经过一系列诸如语义分割、物体识别、物体分割等方法将物理世界以抽象的方式进行表达。 当前HIROP的感知模块仅实现了“前端”。
感知模块的框架
感知模块采用了实现了管道流和图计算的ECTO作为主框架。其中有四个关键的概念:源(Source)、过滤器(Filter)、发布器(Publisher)、图(Graph)。
- 1,当数据从传感器中读出后,会按顺序流向感知模块的不同过滤器(从传感器中获取数据的过滤器就是Source);
- 2,当数据经过过滤器时,过滤器会通过相关相关算法对数据进行修改(这就是Filter的作用);
- 3,当数据流过所有的过滤器后,会通过一个过滤器将数据发布出去(发布数据的特殊过滤器就是Publisher);
- 4,感知模块可以根据需求随意的连接相关过滤器来构成一个图(Graph可以根据需求随意变化)。
简单的Graph
编程
Note
当前感知模块的所有数据均为点云,下面的所有教程均以编写点云的过滤器为例子。
下载源码
HIROP感知模块是作为HIROP的一个子模块存在,因此需要下载完整的HIROP工程。
$ git clone https://github.com/HSRobot/hirop.git
$ cd hirop ; mkdir build; cd build
$ cmake ../
$ make
$ make install
$ echo 'PYTHONPATH=/usr/local/lib/python2.7/dist-packages/hirop_perecption/:$PYTHONPATH' >> ~/.bashrc
编写过滤器
在hirop/src/perception/filter/中创建过滤器的源文件(myfilter.cpp)和头文件(myfilter.h)。
myfilter.h
#pragma once
#include <ecto/ecto.hpp>
#include <ecto_pcl/ecto_pcl.hpp>
using ecto::tendrils;
namespace hirop_perception{
class MyFilter{
public:
/**
* @brief MyFilter 构造函数
*/
MyFilter(){}
/**
* 析构函数
*/
~MyFilter(){}
/**
* @brief 定义过滤器参数
*/
static void declare_params(tendrils& params);
/**
* @brief 定义过滤器输入输出
*/
static void declare_io(const tendrils& params, tendrils& in, tendrils& out);
/**
* @brief 过滤器的过滤实现
*/
template <typename Point>
int process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const pcl::PointCloud<Point> >& input);
/**
* @brief 过滤器的配置
*/
void configure(const tendrils& params, const tendrils& inputs, const tendrils& outputs);
private:
float _z;
};
}
myfilter.cpp
#include "myfilter.h"
using namespace hirop_perception;
void MyFilter::declare_params(tendrils& params){
params.declare<float>("Z", "The pointcolud vaild Z area", 5);
}
void MyFilter::declare_io(const ecto::tendrils ¶ms, ecto::tendrils &in, ecto::tendrils &out){
in.declare<ecto::pcl::PointCloud>("input", "The colud to filter");
out.declare<ecto::pcl::PointCloud>("output", "The colud to out");
}
void MyFilter::configure(const ecto::tendrils ¶ms, const ecto::tendrils &inputs, const ecto::tendrils &outputs){
_z = params.get<float>("Z");
}
template<typename Point>
int MyFilter::process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const pcl::PointCloud<Point> >& input){
typename pcl::PointCloud<Point>::Ptr outPointCloud(new pcl::PointCloud<Point>);
int size = input->points.size();
for(int i =0; i < size; i++){
if(input->points[i].z < _z)
outPointCloud->points.push_back(cloud->points[i]);
}
outPointCloud->height = 1;
outPointCloud->width = outPointCloud->points.size();
outputs.get<ecto::pcl::PointCloud>("output") = ecto::pcl::PointCloud(outPointCloud);
return ecto::OK;
}
ECTO_CELL(hirop_perception, ecto::pcl::PclCell<MyFilter>,\
"VoxelFilter", "The point cloud voxel filter")
代码解析
必要的头文件
我们需要在filter.h中导入必要的头文件
#include <ecto/ecto.hpp>
#include <ecto_pcl/ecto_pcl.hpp>
过滤器的关键函数
- declare_params函数:
void MyFilter::declare_params(tendrils& params){
params.declare<float>("Z", "The pointcolud vaild Z area", 5);
}
该函数用于声明过滤器的参数。在下面的代码里,我们声明了MyFilter这个过滤器有个名为Z
,类型为float
的参数。用于配置过滤器应当过滤什么范围以外的数据。
- declare_io函数:
void MyFilter::declare_io(const ecto::tendrils ¶ms, ecto::tendrils &in, ecto::tendrils &out){
in.declare<ecto::pcl::PointCloud>("input", "The colud to filter");
out.declare<ecto::pcl::PointCloud>("output", "The colud to out");
}
该函数用于声明过滤器的输入和输出。每个过滤器均有输入和输出。在本例中我们有一个名为input
,类型为ecto::pcl::PointCloud
的输入,用于从上一级过滤器中介绍点云。 以及一个名为output
,类型为ecto::pcl::PointCloud
的输出,用于将点云输出到下一级过滤器中。
- configure函数:
void MyFilter::configure(const ecto::tendrils ¶ms, const ecto::tendrils &inputs, const ecto::tendrils &outputs){
_z = params.get<float>("Z");
}
该函数的作用在于:当过滤器被初始化时来完成过滤器内部的初始化,比如处理相关参数。在本例中,configure函数只是将过滤器参数Z
保存起来而已。
- process函数:
template<typename Point>
int MyFilter::process(const tendrils& inputs, const tendrils& outputs,
boost::shared_ptr<const pcl::PointCloud<Point> >& input){
typename pcl::PointCloud<Point>::Ptr outPointCloud(new pcl::PointCloud<Point>);
int size = input->points.size();
for(int i =0; i < size; i++){
if(input->points[i].z < _z)
outPointCloud->points.push_back(cloud->points[i]);
}
outPointCloud->height = 1;
outPointCloud->width = outPointCloud->points.size();
outputs.get<ecto::pcl::PointCloud>("output") = ecto::pcl::PointCloud(outPointCloud);
return ecto::OK;
}
整个过滤器最重要的函数,用于对数据进行处理。在本例中,我们只是简单的将Z轴大于_z
的点云给丢弃。然后通过下面的两行代码将过滤后的点云输出至下一级过滤器中。
outputs.get<ecto::pcl::PointCloud>("output") = ecto::pcl::PointCloud(outPointCloud);
return ecto::OK;
导出过滤器
最后,我们需要通过下面的代码来导出我们刚刚完成的过滤器。
ECTO_CELL(hirop_perception, ecto::pcl::PclCell<MyFilter>,\
"MyFilter", "My first filter")
ECTO_CELL
宏的参数描述: - hirop_perception:生成的感知模块的名称 - ecto::pcl::PclCell:声明实习过滤器功能的类 - "MyFilter":过滤器的名称 - "My first filter":过滤器的描述
配置工程
在完成代码的编写后,我们还需要配置相关的工程来将我们的过滤器编译。 在hirop/src/perception/filter/CMakeLists.txt的ectomodule函数前中添加以下代码
SET(SOURCES
${SOURCES}
myfilter.cpp)
编译工程
$ cd hirop/build
$ make
$ sudo make install
测试过滤器
在完成上述的教程后,我们就可以来测试下我们的过滤器,看看其是否有效。首先让我们创建一个python文件:perception_test.py,输入以下内容:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import hirop_perception
import rospy
import ecto, ecto_pcl, ecto_ros
if __name__=="__main__":
# 初始化ROS节点
ecto_ros.init(sys.argv, "perception_test")
rospy.init_node('ros_test')
# 构造一个从ROS话题中获取数据的过滤器,第一个参数是过滤器的名称,第二个参数是话题名称,第三个是将点云转换到的目标坐标系
rosSource = hirop_perception.PointCloudRos('source', topic_name="/camera/depth/points", world_frame='base_link')
# 构造一个将过滤器输出发布到ROS话题的过滤器,第一个参数是过滤器的名称,第二参数是发布的点云的参考坐标系
publisher = hirop_perception.PointCloudPublish("publisher", frame_id="base_link")
# 构造一个我们刚刚制作的自己的过滤器,设置过滤距离为2米
myFilter = hirop_perception.MyFilter('myFilter', Z=2)
# 构造一个点云可视化窗口过滤器
pclViewer = ecto_pcl.CloudViewer("viewer", window_name="PCD Viewer")
# 声明一个计算图
testPlasm = ecto.Plasm()
# 定义计算图的结构
testPlasm.connect(rosSource["output"] >> self.myFilter["input"], self.myFilter["output"] >> self.pclViewer["input"], self.pclFunsion["output"] >> self.publisher["input"])
# 运行计算图
sched = ecto.Scheduler(lookPlasm)
# 进入ROS的消息循环
rospy.spin()
直接运行脚本后,在接收到点云的时候会弹出一个窗口,正常的情况下应该可以看到Z大于2m的点云全部被过滤掉了。
完成
至此,所有教程均已结束
环境搭建指南汇总
该文档汇总了在HIROP
平台所依赖的第三方驱动、软件包等的安装教程。在这里你可以找到大部分较复杂软件的安装教程。
列表
ros_qtc_plugin环境搭建
ros_qtc_plugin编译
ROS下编译qtc插件
准备
在编译qtc插件之前,需要准备下载源码
Warning
需要注意两个源码的版本需要对应,建议使用系统中的QT版本所对应的QT Creator版本
编译
在编译之前,我们需要注意以下事项
- QT Creator和ros_qtc_plugin的源码应当放在同一目录下
- QT Creator的源码目录名应当为
qt-creator
- QT Creator的编译目录名应当为
qt-creator-build
编译 lxqt-build-tools
$ git clone https://github.com/lxqt/lxqt-build-tools.git
$ cd lxqt-build-tools
$ cmake ./
$ sudo make install
编译 qtermwidget
$ git clone https://github.com/lxqt/qtermwidget.git
$ cd qtermwidget
$ cmake ./
$ make; sudo make install
编译ros_qtc_plugin插件
使用qtcreator进行编译即可
问题汇总
该文档汇总了在HIROP
开发过程中使用了的必要性的文档,包含相关API手册,第三方博客等外部链接。
资源列表
问题汇总
该文档汇总了在HIROP
开放过程中遇到的典型问题,在开发者在开发过程中遇到相关问题时可以给予相关的经验。