search

TianyeAlex / tracker_kcf_ros

基于ros下应用深度相机的kcf追踪算法实现
137 stars 55 forks source link

turtlebot2 无法跟随目标 #2

Open kkycj opened 6 years ago

kkycj commented 6 years ago

我使用的是turtlebot2，已经启动了kobuki的底座，运行节点以后能够在窗口中对框选目标进行跟踪但是机器人不会跟随是什么原因？终端可以看到一直有线性速度和角速度在刷新，但是小车完全不会动？

TianyeAlex commented 6 years ago

你是通过usb串口通信的吗，是不是串口权限有问题

kkycj commented 6 years ago

谢谢 问题已经解决了，turtlebot2要控制底座必须把/tracker/cmd_vel的topic重映射到/cmd_vel_mux/input/teleop，launch文件里加一句remap就可以了。

MengyaXu commented 6 years ago

我也遇到了相同的问题，请问可以详细点解答吗？remap写在哪里啊？刚接触机器人不久，不是很熟悉。。 感激不尽！

kkycj commented 6 years ago

remap写在程序包下面的launch文件里，或者直接在启动节点的时候加上重映射的命令也可以。

<launch> <node pkg="track_pkg" type="kcf_node" name="kcf_tracker" output="screen"> <remap from="tracker/cmd_vel" to="cmd_vel_mux/input/teleop"/> </node> </launch>

我的launch文件是这么写的，你可以参考一下。或者不嫌麻烦启动节点用这个命令： $rosrun track_pkg kcf_node /tracker/cmd_vel:=/cmd_vel_mux/input/teleop

具体的topic要看你使用的机器人，用topic list查看一下就好。

baogege6661 commented 5 years ago

我运行这个代码，但是turtlebot只能跟随转圈，不能向前向后移动，这是怎么回事，有哪位大佬能够不吝赐教

kkycj commented 5 years ago

@baogege6661 你看一下程序输出的linear_speed有没有读数呢

baogege6661 commented 5 years ago

有啊，线速度和角速度不断的在刷新，我保证turtlebot自身是完好的

kkycj commented 5 years ago

你试一下用键盘teleop控制的时候能不能前后移动，程序本身有线速度输出的话应该不是代码的问题。

baogege6661 commented 5 years ago

不好意思我刚看错了，我的线速度一直为0

，这是为什么呢，是不是代码上关于识别距离的地方有误呢

kkycj commented 5 years ago

那就用rviz或者image_view检查一下kinect的深度图能不能正常读取吧。

baogege6661 commented 5 years ago

image view显示深度是有的，我订阅的是/Kinect2/qhd/image_depth_rect这个深度话题

kkycj commented 5 years ago

@baogege6661 Min_distance有改过设置吗？正常情况下如果距离一直小于Min_distance直线速度就一直是0。

baogege6661 commented 5 years ago

我的最小距离1米，近远距离我都试过了，都是只能旋转不能直行，心累

kkycj commented 5 years ago

@baogege6661 如果你用的是原作者的代码的话，直线速度的计算只是和距离相关一个线性方程，你在程序里加一句cout看一下distance有没有被正确读取吧。

baogege6661 commented 5 years ago

我把原作者的distance注释解开了，但是终端没有打印出来，不知道哪里出了问题

kkycj commented 5 years ago

@baogege6661 可以把你的代码贴上来吗，这样比较难发现你的问题。

baogege6661 commented 5 years ago

include

include

include

include

include

include <ros/ros.h>

include <image_transport/image_transport.h>

include <cv_bridge/cv_bridge.h>

include <sensor_msgs/image_encodings.h>

include "geometry_msgs/Twist.h"

include <opencv2/core/core.hpp>

include <opencv2/highgui/highgui.hpp>

include "kcftracker.hpp"

static const std::string RGB_WINDOW = "RGB Image window"; //static const std::string DEPTH_WINDOW = "DEPTH Image window";

define Max_linear_speed 0.6

define Min_linear_speed 0.4

define Min_distance 0.8

define Max_distance 5.0

define Max_rotation_speed 0.75

float linear_speed = 0; float rotation_speed = 0;

float k_linear_speed = (Max_linear_speed - Min_linear_speed) / (Max_distance - Min_distance); float h_linear_speed = Min_linear_speed - k_linear_speed * Min_distance;

float k_rotation_speed = 0.004; float h_rotation_speed_left = 1.2; float h_rotation_speed_right = 1.36;

int ERROR_OFFSET_X_left1 = 100; int ERROR_OFFSET_X_left2 = 300; int ERROR_OFFSET_X_right1 = 340; int ERROR_OFFSET_X_right2 = 540;

cv::Mat rgbimage; cv::Mat depthimage; cv::Rect selectRect; cv::Point origin; cv::Rect result;

bool select_flag = false; bool bRenewROI = false; // the flag to enable the implementation of KCF algorithm for the new chosen ROI bool bBeginKCF = false; bool enable_get_depth = false;

bool HOG = true; bool FIXEDWINDOW = false; bool MULTISCALE = true; bool SILENT = true; bool LAB = false;

// Create KCFTracker object KCFTracker tracker(HOG, FIXEDWINDOW, MULTISCALE, LAB);

float dist_val[5] ;

void onMouse(int event, int x, int y, int, void*) { if (select_flag) { selectRect.x = MIN(origin.x, x);
selectRect.y = MIN(origin.y, y); selectRect.width = abs(x - origin.x);
selectRect.height = abs(y - origin.y); selectRect &= cv::Rect(0, 0, rgbimage.cols, rgbimage.rows); } if (event == CV_EVENT_LBUTTONDOWN) { bBeginKCF = false;
select_flag = true; origin = cv::Point(x, y);
selectRect = cv::Rect(x, y, 0, 0);
} else if (event == CV_EVENT_LBUTTONUP) { select_flag = false; bRenewROI = true; } }

class ImageConverter { ros::NodeHandle nh_; imagetransport::ImageTransport it; image_transport::Subscriber imagesub; image_transport::Subscriber depthsub;

public: ros::Publisher pub;

ImageConverter() : it(nh) { // Subscrive to input video feed and publish output video feed imagesub = it_.subscribe("/kinect2/qhd/image_color_rect", 1, &ImageConverter::imageCb, this); depthsub = it_.subscribe("/kinect2/hd/image_depthrect/compressed", 1, &ImageConverter::depthCb, this); pub = nh.advertise("tracker/cmd_vel", 1000);

cv::namedWindow(RGB_WINDOW);
//cv::namedWindow(DEPTH_WINDOW);

}

~ImageConverter() { cv::destroyWindow(RGB_WINDOW); //cv::destroyWindow(DEPTH_WINDOW); }

void imageCb(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; }

cv_ptr->image.copyTo(rgbimage);

cv::setMouseCallback(RGB_WINDOW, onMouse, 0);

if(bRenewROI)
{
    // if (selectRect.width <= 0 || selectRect.height <= 0)
    // {
    //     bRenewROI = false;
    //     //continue;
    // }
    tracker.init(selectRect, rgbimage);
    bBeginKCF = true;
    bRenewROI = false;
    enable_get_depth = false;
}

if(bBeginKCF)
{
    result = tracker.update(rgbimage);
    cv::rectangle(rgbimage, result, cv::Scalar( 0, 255, 255 ), 1, 8 );
    enable_get_depth = true;
}
else
    cv::rectangle(rgbimage, selectRect, cv::Scalar(255, 0, 0), 2, 8, 0);

cv::imshow(RGB_WINDOW, rgbimage);
cv::waitKey(1);

}

void depthCb(const sensor_msgs::ImageConstPtr& msg) { cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::TYPE_32FC1); cv_ptr->image.copyTo(depthimage); } catch (cv_bridge::Exception& e) { ROS_ERROR("Could not convert from '%s' to 'TYPE_32FC1'.", msg->encoding.c_str()); }

if(enable_get_depth)
{
  dist_val[0] = depthimage.at<float>(result.y+result.height/3 , result.x+result.width/3) ;
  dist_val[1] = depthimage.at<float>(result.y+result.height/3 , result.x+2*result.width/3) ;
  dist_val[2] = depthimage.at<float>(result.y+2*result.height/3 , result.x+result.width/3) ;
  dist_val[3] = depthimage.at<float>(result.y+2*result.height/3 , result.x+2*result.width/3) ;
  dist_val[4] = depthimage.at<float>(result.y+result.height/2 , result.x+result.width/2) ;

  float distance = 0;
  int num_depth_points = 5;
  for(int i = 0; i < 5; i++)
  {
    if(dist_val[i] > 0.4 && dist_val[i] < 10.0)
      distance += dist_val[i];
    else
      num_depth_points--;
  }
  distance /= num_depth_points;

  //calculate linear speed
  if(distance > Min_distance)
    linear_speed = distance * k_linear_speed + h_linear_speed;
  else
    linear_speed = 0;

  if(linear_speed > Max_linear_speed)
    linear_speed = Max_linear_speed;

  //calculate rotation speed
  int center_x = result.x + result.width/2;
  if(center_x < ERROR_OFFSET_X_left1) 
    rotation_speed =  Max_rotation_speed;
  else if(center_x > ERROR_OFFSET_X_left1 && center_x < ERROR_OFFSET_X_left2)
    rotation_speed = -k_rotation_speed * center_x + h_rotation_speed_left;
  else if(center_x > ERROR_OFFSET_X_right1 && center_x < ERROR_OFFSET_X_right2)
    rotation_speed = -k_rotation_speed * center_x + h_rotation_speed_right;
  else if(center_x > ERROR_OFFSET_X_right2)
    rotation_speed = -Max_rotation_speed;
  else 
    rotation_speed = 0;

  std::cout <<  "linear_speed = " << linear_speed << "  rotation_speed = " << rotation_speed << std::endl;

   std::cout <<  dist_val[0]  << " / " <<  dist_val[1] << " / " << dist_val[2] << " / " << dist_val[3] <<  " / " << dist_val[4] << std::endl;
  std::cout <<  "distance = " << distance << std::endl;
}

cv::imshow(DEPTH_WINDOW, depthimage);
cv::waitKey(1);

} };

int main(int argc, char** argv) { ros::init(argc, argv, "kcf_tracker"); ImageConverter ic;

while(ros::ok())
{
    ros::spinOnce();

geometry_msgs::Twist twist;
twist.linear.x = linear_speed; 
twist.linear.y = 0; 
twist.linear.z = 0;
twist.angular.x = 0; 
twist.angular.y = 0; 
twist.angular.z = rotation_speed;
ic.pub.publish(twist);

    if (cvWaitKey(33) == 'q')
  break;
}

return 0;

}

谢谢，浪费您这么长时间

kkycj commented 5 years ago

@baogege6661 可能是kinect2的图像话题和kinect的图像分辨率不同吧，result的值和深度图的像素坐标没有对上。这个程序是以Kinect一代作相机写的，关于kinect2的变化可能需要自己修改一下。另外你终端不显示距离输出这个我也没看出问题来，不好意思。

baogege6661 commented 5 years ago

非常感谢，我再看看

baogege6661 commented 5 years ago

你好，我发现问题了，我的dist_val数据打印出来很大，基本在1000左右，代码里限定了0.4-10之间，所以distance打印出来为-nan，由于是新手不知道怎么改数值，您能看一下，以下这段代码吗？给出解决方法 if(enable_get_depth) { dist_val[0] = depthimage.at(result.y+result.height/3 , result.x+result.width/3) ; dist_val[1] = depthimage.at(result.y+result.height/3 , result.x+2result.width/3) ; dist_val[2] = depthimage.at(result.y+2result.height/3 , result.x+result.width/3) ; dist_val[3] = depthimage.at(result.y+2result.height/3 , result.x+2result.width/3) ; dist_val[4] = depthimage.at(result.y+result.height/2 , result.x+result.width/2) ;

float distance = 0; int num_depth_points = 5; for(int i = 0; i < 5; i++) { if(dist_val[i] > 0.4 && dist_val[i] < 10.0) distance += dist_val[i]; else num_depth_points--; } distance /= num_depth_points;

//calculate linear speed if(distance > Min_distance) linear_speed = distance * k_linear_speed + h_linear_speed; else linear_speed = 0;

if(linear_speed > Max_linear_speed) linear_speed = Max_linear_speed;

kkycj commented 5 years ago

@baogege6661 应该是单位问题吧 乘上一个比例系数就好了

feifeiwei commented 4 years ago

你好，你的解决了吗？我的也是只能旋转，不能直线走。　　@baogege6661

baogege6661 commented 4 years ago

解决了，我的是深度信息打印出来太大，代码中判断好像是1到10之间。我在那几个深度信息乘以0.01。就解决了。你把深度信息打印出来看是否在判断区间里。整体代码问题是前年默认最短距离单位是米，后面打印距离信息是厘米。单位换算出了问题。具体我也记不清了。

---原始邮件--- 发件人: "feifeiwei"<notifications@github.com> 发送时间: 2019年10月22日(星期二) 下午4:41 收件人: "TianyeAlex/tracker_kcf_ros"<tracker_kcf_ros@noreply.github.com>; 抄送: "Mention"<mention@noreply.github.com>;"baogege6661"<496912993@qq.com>; 主题: Re: [TianyeAlex/tracker_kcf_ros] turtlebot2 无法跟随目标 (#2)

你好，你的解决了吗？我的也是只能旋转，不能直线走。　　@baogege6661

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or unsubscribe.

feifeiwei commented 4 years ago

除以一千转为米确实可以。但是我发现代码使用rgb图的坐标，直接查depth图对应坐标的深度值。然而深度图与彩色图坐标是不对应的。有什么方法吗。@baogege6661 @kkycj

baogege6661 commented 4 years ago

这个我没有深挖，最后效果很不理想。加油，弄好了给我说说

---原始邮件--- 发件人: "feifeiwei"<notifications@github.com> 发送时间: 2019年10月24日(星期四) 中午11:23 收件人: "TianyeAlex/tracker_kcf_ros"<tracker_kcf_ros@noreply.github.com>; 抄送: "Mention"<mention@noreply.github.com>;"baogege6661"<496912993@qq.com>; 主题: Re: [TianyeAlex/tracker_kcf_ros] turtlebot2 无法跟随目标 (#2)

除以一千转为米确实可以。但是我发现代码使用rgb图的坐标，直接查depth图对应坐标的深度值。然而深度图与彩色图坐标是不对应的。。

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub, or unsubscribe.