机器人视觉项目：视觉检测识别+机器人跟随（7）

1.kinect人体骨架追踪中有tf树变换，了解tf树的广播和订阅原理，尝试通过订阅在人体骨架跟踪中tf变换广播的骨架关节点位置坐标信息，获取跟踪目标的位置。

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

src/tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){

  ros::init(argc, argv, "robot_tf_publisher");

  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){

    broadcaster.sendTransform(

      tf::StampedTransform(

        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

        ros::Time::now(),"base_link", "base_laser"));

    r.sleep();

  }

}

src/tf_listener.cpp

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>

#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

  geometry_msgs::PointStamped laser_point;

  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example

  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space

  laser_point.point.x = 1.0;

  laser_point.point.y = 0.2;

  laser_point.point.z = 0.0;

  try{

    geometry_msgs::PointStamped base_point;

    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

        laser_point.point.x, laser_point.point.y, laser_point.point.z,

        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

  }

  catch(tf::TransformException& ex){

    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());

  }

}

int main(int argc, char** argv){

  ros::init(argc, argv, "robot_tf_listener");

  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second

  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

add_executable(tf_broadcaster src/tf_broadcaster.cpp)

add_executable(tf_listener src/tf_listener.cpp)

target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})

target_link_libraries(tf_listener ${catkin_LIBRARIES})

$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

roscore
rosrun robot_setup_tf tf_broadcaster
rosrun robot_setup_tf tf_listener

2.编写目前第一阶段任务细化方案，行人跟随具体实施细节，讨论沟通方案可行性和技术难点。

3.调试机器人的最基础的鼠标画框行人跟随，场景测试。 单个目标手动画框，说着非常low啊..

4.针对鼠标画框行人跟随的测试结果，思考进一步的技术实施方案。目标肯定是要实现自动的进行机器人标定目标，确认跟随，自动开始跟随，
在跟随过程中出现多目标干扰时，能够抗干扰，目标丢失后，能够再次检测到目标，做特征提取和匹配（颜色直方图HOG特征，sift,surf特征...）