项目说明
参考项目:GitHub - TianyeAlex/tracker_kcf_ros: 基于ros下应用深度相机的kcf追踪算法实现
修改内容:
1、解决原作者使用OpenCV版本比较老,部分代码不能正常编译
2、原作者使用RGBD相机获取图像深度信息实现机器人目标追踪,我的项目使用普通话彩色相机通过目标框大小来估算深度
3、增加launch文件,使用remap方式修改话题名
安装
获取源码
cd ~/catkin_ws/src/
git clone https://gitee.com/bingda-robot/kcf_track.git
安装依赖软件
sudo apt install ros-noetic-usb-cam ros-noetic-image-transport
编译
cd ~/catkin_ws/ && catkin_make
使用
启动摄像头,默认使用video0,如果使用其他设备号,可以通过传入device参数
roslaunch kcf_track rgb_camera.launch
启动KCF追踪节点(会打开图像显示框,所以需要在有屏幕的终端例如PC中执行)
roslaunch kcf_track kcf_track.launch
启动后会弹出这样一个图像框
在图像框中使用鼠标框选需要追踪的目标
移动摄像头或者目标,追踪框会跟随目标移动
追踪的同时,节点会根据追踪框中心位置相对图像中心的位置关系计算机器人需要的角速度,根据追踪框当前大小和初始选定的大小关系计算机器人线速度,最终发布cmd_vel话题。
实体机器人上运行追踪
前提:
机器人上已经编译安装了kcf_track
机器人和PC之间已经配置好分布式通信
启动机器人上的摄像头,默认使用video0,如果使用其他设备号,可以通过传入device参数
roslaunch kcf_track rgb_camera.launch
启动机器人底盘节点,例如冰达机器人所使用的base_control
roslaunch base_control base_control.launch
启动KCF追踪节点(会打开图像显示框,所以需要在有屏幕的终端例如PC中执行)
roslaunch kcf_track kcf_track.launch
参数调整
修改kcf_track/src/runtracker.cpp文件中imageCb函数中关于中心点、大小比例和速度的换算关系
distance = (float)(selectRect.width ) / (float)(result.width);
center_x = result.x + result.width/2;
std::cout << "Center x = " << center_x << " Distance = " << distance << std::endl;
std::cout << "Height = " << image_height << " width = " << image_width << std::endl;
//calculate linear speed
if(abs(distance - 1.0) > 0.1)
linear_speed = (distance - 1.0) / 2.0;
else
linear_speed = 0;
if(linear_speed > Max_linear_speed)
linear_speed = Max_linear_speed;
if(linear_speed < -Max_linear_speed)
linear_speed = -Max_linear_speed;
if (abs(float(image_width/2 - center_x))/(image_width/2) > 0.1)
rotation_speed = float(image_width/2 - center_x)/(image_width/2);
else
rotation_speed = 0;
if(rotation_speed > Max_rotation_speed)
rotation_speed = Max_rotation_speed;
if(rotation_speed < -Max_rotation_speed)
rotation_speed = -Max_rotation_speed;
}
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)