# Kinect skeletal follower

Hi, the code i use is referring to the tf tutorial http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20(C%2B%2B).

It works well in the simulator (arbotix), however, when i am using the real turtlebot (kobuki), it doesn't perform smoothly, often lose the target, any idea what is my problem?

this is my openni_tracker launch file. i used `static_transform_publisher`

to introduce new tf which having same position with `/skeletal/torso_1`

and same orientation with `/camera_depth_frame`

. this imitate the relationship between turtle1(/torso) and turtle2(/camera_depth_frame) in the tutorial.

```
<launch>
<node pkg="openni_tracker" name="openni_tracker" type="openni_tracker" output="screen">
<param name="tf_prefix" value="skeleton" />
</node>
<node pkg="tf" type="static_transform_publisher" name="kinect_to_skeletal" args="0 0 0 0 0 0 /camera_depth_frame /skeleton/openni_depth_frame 100" />
<node pkg="tf" type="static_transform_publisher" name="skeletal_to_target" args="0 0 0 -1.57 -1.57 0 /skeleton/torso_1 /target 100" />
</launch>
```

this is my follower code:

```
#include "ros/ros.h"
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
ros::Publisher cmdpub_;
tf::TransformBroadcaster *br;
tf::Transform *tftransform;
tf::TransformListener *ten;
int main(int argc, char** argv){
ros::init(argc, argv, "ar_pose_follower");
ros::NodeHandle nh;
br = new tf::TransformBroadcaster();
tftransform = new tf::Transform();
ten = new tf::TransformListener();
cmdpub_ = nh.advertise<geometry_msgs::Twist> ("cmd_vel", 1); //cmd_vel_mux/input/navi
//markersub_= nh.subscribe("ar_pose_marker", 1, &Listener::markercb, &marker);
while (ros::ok())
{
geometry_msgs::TwistPtr cmd(new geometry_msgs::Twist());
tf::StampedTransform transform;
try {
ten->waitForTransform("/camera_depth_frame", "/target", ros::Time(0), ros::Duration(10.0) );
ten->lookupTransform("/camera_depth_frame", "/target", ros::Time(0), transform);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
cmd->angular.z = 4 * atan2(transform.getOrigin().y(), transform.getOrigin().x()); //<1
//cmd->linear.x = 0.01 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2)); //<1.5
//ROS_INFO("I see transform x, y: %f %f", transform.getOrigin().x(), transform.getOrigin().y());
if (std::abs(cmd->linear.x) < 1.5 && std::abs(cmd->angular.z) < 1){
if (std::abs(cmd->angular.z) > 0.2){
cmd->linear.x = 0.01 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
cmdpub_.publish(cmd);
}
else{
cmd->linear.x = 0.1 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
cmdpub_.publish(cmd);
}
ROS_INFO("I see cmd x, z: %f %f", cmd->linear.x, cmd->angular.z);
}
}
ros::spin();
return 0;
};
```

please help. btw, is there any similar succeeded code i can refer to?