int main(int argc, char** argv)
{
ros::init(argc, argv, "skeleton_tracker");
ros::NodeHandle np("~");
string filepath;
bool is_a_recording;
np.getParam("load_filepath", filepath);
np.param<bool>("load_recording", is_a_recording, false);
g_skeleton_tracker.init();
g_kinect_controller.init(filepath.c_str(), is_a_recording);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
glutCreateWindow ("NITE Skeleton Tracker");
glutKeyboardFunc(glutKeyboard);
glutDisplayFunc(glutDisplay);
glutIdleFunc(glutIdle);
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glutMainLoop();
g_kinect_controller.shutdown();
return 0;
}
void glutDisplay (void)
{
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
g_kinect_controller.getContext().WaitAndUpdateAll();
g_skeleton_tracker.processKinect(g_kinect_controller);
g_kinect_controller.getDepthGenerator().GetMetaData(depthMD);
g_kinect_controller.getUserGenerator().GetUserPixels(0, sceneMD);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
glDisable(GL_TEXTURE_2D);
kinect_display_drawDepthMapGL(depthMD, sceneMD);
kinect_display_drawSkeletonGL(g_kinect_controller.getUserGenerator(),
g_kinect_controller.getDepthGenerator());
glutSwapBuffers();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "skeleton_tracker");
//SkeletonTracker skd;
ros::NodeHandle n;
ros::NodeHandle np("~");
string filepath;
bool is_a_recording;
np.getParam("load_filepath", filepath);
np.param<bool>("load_recording", is_a_recording, false);
g_skeleton_tracker.init();
g_kinect_controller.init(filepath.c_str(), is_a_recording);
// TODO... read these values from ini file
const int fps = 30;
const int width = 640;
const int height = 480;
g_attention_map.init(fps, width, height);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
glutInitWindowSize(GL_WIN_SIZE_X, GL_WIN_SIZE_Y);
depth_window = glutCreateWindow ("Skeleton Depth");
glutDisplayFunc(glutDepthDisplay);
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glutKeyboardFunc(glutKeyboard);
rgb_window = glutCreateWindow ("Skeleton RGB");
glutDisplayFunc(glutRgbDisplay);
glDisable(GL_DEPTH_TEST);
glEnable(GL_TEXTURE_2D);
glEnableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glutIdleFunc(glutIdle);
glutMainLoop();
g_kinect_controller.shutdown();
ros::spin();
return 0;
}
void glutIdle (void)
{
g_kinect_controller.getContext().WaitAndUpdateAll();
g_skeleton_tracker.processKinect(g_kinect_controller);
glutSetWindow(depth_window);
glutPostRedisplay();
glutSetWindow(rgb_window);
glutPostRedisplay();
}
void publishTransform(KinectController &kinect_controller, XnUserID const &user, XnSkeletonJoint const &joint, string const &frame_id, string const &child_frame_id, skeleton_markers::Skeleton &skeleton)
{
xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
static tf::TransformBroadcaster br;
XnSkeletonJointPosition joint_position;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, joint_position);
double x = joint_position.position.X / 1000.0;
double y = joint_position.position.Y / 1000.0;
double z = joint_position.position.Z / 1000.0;
XnSkeletonJointOrientation joint_orientation;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, joint, joint_orientation);
XnFloat* m = joint_orientation.orientation.elements;
KDL::Rotation rotation(m[0], m[1], m[2],
m[3], m[4], m[5],
m[6], m[7], m[8]);
double qx, qy, qz, qw;
rotation.GetQuaternion(qx, qy, qz, qw);
geometry_msgs::Vector3 position;
geometry_msgs::Quaternion orientation;
position.x = x;
position.y = y;
position.z = z;
orientation.x = qx;
orientation.y = qy;
orientation.z = qz;
orientation.w = qw;
skeleton.name.push_back(child_frame_id);
skeleton.position.push_back(position);
skeleton.orientation.push_back(orientation);
skeleton.confidence.push_back(joint_position.fConfidence);
tf::Transform transform;
transform.setOrigin(tf::Vector3(x, y, z));
transform.setRotation(tf::Quaternion(qx, qy, qz, qw));
tf::Transform change_frame;
change_frame.setOrigin(tf::Vector3(0, 0, 0));
tf::Quaternion frame_rotation;
frame_rotation.setEulerZYX(1.5708, 0, 1.5708);
change_frame.setRotation(frame_rotation);
transform = change_frame * transform;
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), frame_id, child_frame_id));
}
void glutRgbDisplay (void)
{
xn::ImageMetaData imageMD;
g_kinect_controller.getImageGenerator().GetMetaData(imageMD);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0, imageMD.XRes(), imageMD.YRes(), 0, -1.0, 1.0);
glDisable(GL_TEXTURE_2D);
kinect_display_drawRgbMapGL(imageMD, g_attention_map);
kinect_display_drawSkeletonGL(g_kinect_controller.getUserGenerator(),
g_kinect_controller.getDepthGenerator(), false);
glutSwapBuffers();
}
void processKinect(KinectController &kinect_controller)
{
XnUserID users[15];
XnUInt16 users_count = 15;
xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
UserGenerator.GetUsers(users, users_count);
pi_tracker::Skeleton g_skel;
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!UserGenerator.GetSkeletonCap().IsTracking(user))
continue;
publishTransform(kinect_controller, user, XN_SKEL_HEAD, fixed_frame, "head", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_NECK, fixed_frame, "neck", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_TORSO, fixed_frame, "torso", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_SHOULDER, fixed_frame, "left_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_ELBOW, fixed_frame, "left_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HAND, fixed_frame, "left_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_SHOULDER, fixed_frame, "right_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_ELBOW, fixed_frame, "right_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HAND, fixed_frame, "right_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HIP, fixed_frame, "left_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_KNEE, fixed_frame, "left_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_FOOT, fixed_frame, "left_foot", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HIP, fixed_frame, "right_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_KNEE, fixed_frame, "right_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_FOOT, fixed_frame, "right_foot", g_skel);
g_skel.user_id = user;
g_skel.header.stamp = ros::Time::now();
g_skel.header.frame_id = fixed_frame;
skeleton_pub_.publish(g_skel);
break; // only read first user
}
}
void processKinect(KinectController &kinect_controller)
{
XnUserID users[3];
XnUInt16 users_count = 3;
xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
xn::DepthGenerator& depthGenerator = kinect_controller.getDepthGenerator();
UserGenerator.GetUsers(users, users_count);
skeleton_markers::Skeleton g_skel;
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!UserGenerator.GetSkeletonCap().IsTracking(user)){
continue;
}
XnPoint3D com;
UserGenerator.GetCoM(user, com);
depthGenerator.ConvertRealWorldToProjective(1, &com, &com);
glVertex2f(com.X, com.Y);
// Calculate the distance between human and camera
float dist = com.Z /1000.0f;
geometry_msgs::Twist vel;
if(g_LeftHandPositionHistory.Speed()>150)
{
vel.linear.x = 0.00;
vel.angular.z = -0.08;
ROS_INFO("Robot is moving anticlockwise %.2f", g_LeftHandPositionHistory.Speed());
}
else if(g_RightHandPositionHistory.Speed()>150 && g_LeftHandPositionHistory.Speed()>150)
{
ROS_INFO("Robot is moving....");
ros::Duration(1.0).sleep(); // sleep for 1 second
ROS_INFO("Robot.....");
}
else if(g_RightHandPositionHistory.Speed()>150){
vel.linear.x = 0.00;
vel.angular.z = 0.08;
ROS_INFO("Robot is moving clockwise");
}
else{
//velocity command send to robot for follow human.
if(dist >=1.5)
{
ROS_INFO("I am following you");
vel.linear.x = 0.1;
vel.angular.z = 0.0;
}
else if(dist <=1.5 && dist >=1.0)
{
ROS_INFO("I am rounding ...");
vel.linear.x = 0.0;
vel.angular.z = 0.1;
}
else{
ROS_INFO(" You are too near to me");
vel.linear.x = -0.1;
vel.angular.z = 0.0;
}
}
handtrajectory(UserGenerator, depthGenerator, user, XN_SKEL_RIGHT_HAND, true);
handtrajectory(UserGenerator, depthGenerator, user, XN_SKEL_LEFT_HAND, true);
publishTransform(kinect_controller, user, XN_SKEL_HEAD, fixed_frame, "head", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_NECK, fixed_frame, "neck", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_TORSO, fixed_frame, "torso", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_SHOULDER, fixed_frame, "left_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_ELBOW, fixed_frame, "left_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HAND, fixed_frame, "left_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_SHOULDER, fixed_frame, "right_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_ELBOW, fixed_frame, "right_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HAND, fixed_frame, "right_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HIP, fixed_frame, "left_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_KNEE, fixed_frame, "left_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_FOOT, fixed_frame, "left_foot", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HIP, fixed_frame, "right_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_KNEE, fixed_frame, "right_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_FOOT, fixed_frame, "right_foot", g_skel);
// Input Joint Positions And Orientations from kinect
// Upper Joints positions
XnSkeletonJointPosition joint_position_head;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_HEAD, joint_position_head);
KDL::Vector head(joint_position_head.position.X, joint_position_head.position.Y, joint_position_head.position.Z);
XnSkeletonJointPosition joint_position_neck;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_NECK, joint_position_neck);
KDL::Vector neck(joint_position_neck.position.X, joint_position_neck.position.Y, joint_position_neck.position.Z);
XnSkeletonJointPosition joint_position_torso;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_TORSO, joint_position_torso);
KDL::Vector torso(joint_position_torso.position.X, joint_position_torso.position.Y, joint_position_torso.position.Z);
// Left Arm ****
XnSkeletonJointPosition joint_position_left_hand;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HAND, joint_position_left_hand);
KDL::Vector left_hand(joint_position_left_hand.position.X, joint_position_left_hand.position.Y, joint_position_left_hand.position.Z);
XnSkeletonJointPosition joint_position_left_elbow;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, joint_position_left_elbow);
KDL::Vector left_elbow(joint_position_left_elbow.position.X, joint_position_left_elbow.position.Y, joint_position_left_elbow.position.Z);
XnSkeletonJointPosition joint_position_left_shoulder;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, joint_position_left_shoulder);
KDL::Vector left_shoulder(joint_position_left_shoulder.position.X, joint_position_left_shoulder.position.Y, joint_position_left_shoulder.position.Z);
// Right Arm ****
XnSkeletonJointPosition joint_position_right_hand;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_HAND, joint_position_right_hand);
KDL::Vector right_hand(joint_position_right_hand.position.X, joint_position_right_hand.position.Y, joint_position_right_hand.position.Z);
XnSkeletonJointPosition joint_position_right_elbow;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_ELBOW, joint_position_right_elbow);
KDL::Vector right_elbow(joint_position_right_elbow.position.X, joint_position_right_elbow.position.Y, joint_position_right_elbow.position.Z);
XnSkeletonJointPosition joint_position_right_shoulder;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, joint_position_right_shoulder);
KDL::Vector right_shoulder(joint_position_right_shoulder.position.X, joint_position_right_shoulder.position.Y, joint_position_right_shoulder.position.Z);
// Right Leg ****
XnSkeletonJointPosition joint_position_right_hip;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_HIP, joint_position_right_hip);
KDL::Vector right_hip(joint_position_right_hip.position.X, joint_position_right_hip.position.Y, joint_position_right_hip.position.Z);
XnSkeletonJointPosition joint_position_right_knee;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_KNEE, joint_position_right_knee);
KDL::Vector right_knee(joint_position_right_knee.position.X, joint_position_right_knee.position.Y, joint_position_right_knee.position.Z);
XnSkeletonJointPosition joint_position_right_foot;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_FOOT, joint_position_right_foot);
KDL::Vector right_foot(joint_position_right_foot.position.X, joint_position_right_foot.position.Y, joint_position_right_foot.position.Z);
// Left Leg ****
XnSkeletonJointPosition joint_position_left_hip;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HIP, joint_position_left_hip);
KDL::Vector left_hip(joint_position_left_hip.position.X, joint_position_left_hip.position.Y, joint_position_left_hip.position.Z);
XnSkeletonJointPosition joint_position_left_knee;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_KNEE, joint_position_left_knee);
KDL::Vector left_knee(joint_position_left_knee.position.X, joint_position_left_knee.position.Y, joint_position_left_knee.position.Z);
XnSkeletonJointPosition joint_position_left_foot;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_FOOT, joint_position_left_foot);
KDL::Vector left_foot(joint_position_left_foot.position.X, joint_position_left_foot.position.Y, joint_position_left_foot.position.Z);
// Upper joints rotations
XnSkeletonJointOrientation joint_orientation_head;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_HEAD, joint_orientation_head);
XnFloat* h = joint_orientation_head.orientation.elements;
KDL::Rotation head_rotation(h[0], h[1], h[2],
h[3], h[4], h[5],
h[6], h[7], h[8]);
double head_roll, head_pitch, head_yaw;
head_rotation.GetRPY(head_roll, head_pitch, head_yaw);
// head yaw
static double head_angle_yaw = 0;
if (joint_orientation_head.fConfidence >= 0.5)
{
head_angle_yaw = head_pitch;
}
// head pitch
static double head_angle_pitch = 0;
if (joint_orientation_head.fConfidence >= 0.5)
{
head_angle_pitch = head_roll;
}
XnSkeletonJointOrientation joint_orientation_neck;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_NECK, joint_orientation_neck);
XnFloat* n = joint_orientation_neck.orientation.elements;
KDL::Rotation neck_rotation(n[0], n[1], n[2],
n[3], n[4], n[5],
n[6], n[7], n[8]);
double neck_roll, neck_pitch, neck_yaw;
neck_rotation.GetRPY(neck_roll, neck_pitch, neck_yaw);
XnSkeletonJointOrientation joint_orientation_torso;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_TORSO, joint_orientation_torso);
XnFloat* t = joint_orientation_torso.orientation.elements;
KDL::Rotation torso_rotation(t[0], t[1], t[2],
t[3], t[4], t[5],
t[6], t[7], t[8]);
double torso_roll, torso_pitch, torso_yaw;
torso_rotation.GetRPY(torso_roll, torso_pitch, torso_yaw);
// torso yaw
static double torso_angle_yaw = 0;
if (joint_orientation_torso.fConfidence >= 0.5)
{
torso_angle_yaw = torso_pitch;
}
// the kinect and robot are in different rotation spaces,
// Process and output joint rotations to the robot.
// ARMS for robot
// left elbow roll ****
KDL::Vector left_elbow_hand(left_hand - left_elbow);
KDL::Vector left_elbow_shoulder(left_shoulder - left_elbow);
left_elbow_hand.Normalize();
left_elbow_shoulder.Normalize();
static double left_elbow_angle_roll = 0;
if (joint_position_left_hand.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5 &&
joint_position_left_shoulder.fConfidence >= 0.5)
{
left_elbow_angle_roll = acos(KDL::dot(left_elbow_hand, left_elbow_shoulder));
left_elbow_angle_roll = left_elbow_angle_roll - PI;
}
// right elbow roll ****
KDL::Vector right_elbow_hand(right_hand - right_elbow);
KDL::Vector right_elbow_shoulder(right_shoulder - right_elbow);
right_elbow_hand.Normalize();
right_elbow_shoulder.Normalize();
static double right_elbow_angle_roll = 0;
if (joint_position_right_hand.fConfidence >= 0.5 &&
joint_position_right_elbow.fConfidence >= 0.5 &&
joint_position_right_shoulder.fConfidence >= 0.5)
{
right_elbow_angle_roll = acos(KDL::dot(right_elbow_hand, right_elbow_shoulder));
right_elbow_angle_roll = -(right_elbow_angle_roll - PI);
}
// left shoulder roll ****
KDL::Vector left_shoulder_elbow(left_elbow - left_shoulder);
KDL::Vector left_shoulder_right_shoulder(right_shoulder - left_shoulder);
left_shoulder_elbow.Normalize();
left_shoulder_right_shoulder.Normalize();
static double left_shoulder_angle_roll = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5 &&
joint_position_left_shoulder.fConfidence >= 0.5)
{
left_shoulder_angle_roll = acos(KDL::dot(left_shoulder_elbow, left_shoulder_right_shoulder));
left_shoulder_angle_roll = left_shoulder_angle_roll - HALFPI;
}
// right shoulder roll ****
KDL::Vector right_shoulder_elbow(right_elbow - right_shoulder);
KDL::Vector right_shoulder_left_shoulder(left_shoulder - right_shoulder);
right_shoulder_elbow.Normalize();
right_shoulder_left_shoulder.Normalize();
static double right_shoulder_angle_roll = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5 &&
joint_position_right_elbow.fConfidence >= 0.5 &&
joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_roll = acos(KDL::dot(right_shoulder_elbow, right_shoulder_left_shoulder));
right_shoulder_angle_roll = -(right_shoulder_angle_roll - HALFPI);
}
// left shoulder pitch ****
static double left_shoulder_angle_pitch = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5)
{
left_shoulder_angle_pitch = asin(left_shoulder_elbow.y());
left_shoulder_angle_pitch = left_shoulder_angle_pitch + HALFPI;
}
// right shoulder pitch ****
static double right_shoulder_angle_pitch = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_pitch = asin(right_shoulder_elbow.y());
right_shoulder_angle_pitch = -(right_shoulder_angle_pitch + HALFPI);
}
// left shoulder yaw ****
static double left_shoulder_angle_yaw = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5)
{
left_shoulder_angle_yaw = asin(left_elbow_hand.x());
}
// right shoulder yaw ****
static double right_shoulder_angle_yaw = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_yaw = asin(right_elbow_hand.x());
right_shoulder_angle_yaw = -(right_shoulder_angle_yaw);
}
// Use head for counter balancing
static double hp_min = -0.1;
static double hp_max = 0.65;
// if this number is close to zero head should be hp_min
// if it is close to or above HALFPI then head should be hp_max
double arm_pitch_factor = (fabs(right_shoulder_angle_pitch) +
fabs(left_shoulder_angle_pitch)) / 2.0;
if (arm_pitch_factor >= HALFPI)
head_angle_pitch = 0.65;
else
head_angle_pitch = hp_min + ((arm_pitch_factor / HALFPI) * (hp_max - hp_min));
// head follows arm most sideways
double arm_roll_factor = (right_shoulder_angle_roll +
left_shoulder_angle_roll) / 2.0;
head_angle_yaw = arm_roll_factor * -1.1;
sensor_msgs::JointState js;
js.name.push_back("elbow_left_roll");
js.position.push_back(left_elbow_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_roll");
js.position.push_back(left_shoulder_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_pitch");
js.position.push_back(left_shoulder_angle_pitch);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_yaw");
js.position.push_back(left_shoulder_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("elbow_right_roll");
js.position.push_back(right_elbow_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_roll");
js.position.push_back(right_shoulder_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_pitch");
js.position.push_back(right_shoulder_angle_pitch);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_yaw");
js.position.push_back(right_shoulder_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("neck_yaw");
js.position.push_back(head_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("neck_pitch");
js.position.push_back(head_angle_pitch);
js.velocity.push_back(10);
joint_states_pub_.publish(js);
/////
// Following is IK method
/////
KDL::Vector left_hand_torso = left_hand; // - torso;
KDL::Vector right_hand_torso = right_hand; // - torso;
double scale = 0.0003432;
left_hand_torso = scale * left_hand_torso;
right_hand_torso = scale * right_hand_torso;
geometry_msgs::Point p;
if (joint_position_left_hand.fConfidence >= 0.5)
{
p.x = -left_hand_torso.z();
p.y = -left_hand_torso.x();
p.z = left_hand_torso.y();
left_arm_destination_pub_.publish(p);
}
if (joint_position_right_hand.fConfidence >= 0.5)
{
p.x = -right_hand_torso.z();
p.y = -right_hand_torso.x();
p.z = right_hand_torso.y();
right_arm_destination_pub_.publish(p);
}
// The robot will receive and try to mirror my position
geometry_msgs::Point torso_destination;
torso_destination.x = -torso.z() / 1000.0;
torso_destination.y = -torso.x() / 1000.0;
//robot_destination.z = torso.y() / 1000.0;
torso_destination.z = torso_pitch;
torso_destination_pub_.publish(torso_destination);
g_skel.user_id = user;
g_skel.header.stamp = ros::Time::now();
g_skel.header.frame_id = fixed_frame;
skeleton_pub_.publish(g_skel);
cmdVelPub.publish(vel);
//Visualize the trajectory of right hand in RVIZ
visualization_msgs::Marker points;
points.header.frame_id = "/openni_depth_frame";
points.header.stamp = ros::Time::now();
points.ns = "trajectory_right";
points.type = visualization_msgs::Marker::POINTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
points.scale.x = 0.03;
points.scale.y = 0.03;
points.color.r = 1.0;
points.color.a = 0.5;
XnPoint3D pt;
for (int k = 0; k < g_RightHandPositionHistory.Size(); ++k) {
g_RightHandPositionHistory.GetValueScreen (k, pt);
geometry_msgs::Point p;
p.x = pt.Z/1000;
p.y = pt.Y/1000;
p.z = pt.X/1000;
points.points.push_back(p);
}
marker_pub.publish(points);
//Visualize the trajectory of left hand in RVIZ
visualization_msgs::Marker point;
point.header.frame_id = "/openni_depth_frame";
point.header.stamp = ros::Time::now();
point.ns = "trajectory_left";
point.type = visualization_msgs::Marker::POINTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
point.scale.x = 0.03;
point.scale.y = 0.03;
point.color.b = 1.0;
point.color.a = 0.5;
XnPoint3D pt1;
for (int k = 0; k < g_LeftHandPositionHistory.Size(); ++k) {
g_LeftHandPositionHistory.GetValueScreen (k, pt1);
geometry_msgs::Point p;
p.x = pt1.Z/1000;
p.y = pt1.Y/1000;
p.z = pt1.X/1000;
point.points.push_back(p);
}
marker_pub.publish(point);
break; // only read first user
}
}
void getSkeleton (void)
{
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
g_kinect_controller.getContext().WaitAndUpdateAll();
g_skeleton_tracker.processKinect(g_kinect_controller);
g_kinect_controller.getDepthGenerator().GetMetaData(depthMD);
g_kinect_controller.getUserGenerator().GetUserPixels(0, sceneMD);
/* ADDED */
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
(g_kinect_controller.getUserGenerator()).GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if ((g_kinect_controller.getUserGenerator()).GetSkeletonCap().IsTracking(aUsers[i]))
{
// LEFT
XnSkeletonJointPosition joint1, joint2;
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, joint1);
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, joint2);
if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
g_kinect_controller.getDepthGenerator().ConvertRealWorldToProjective(2, pt, pt);
std_msgs::Int32MultiArray l_array;
l_array.data.clear();
l_array.data.push_back(pt[1].X);
l_array.data.push_back(pt[1].Y);
l_hand_pub.publish(l_array);
// RIGHT
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, joint1);
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, joint2);
if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
{
return;
}
pt[0] = joint1.position;
pt[1] = joint2.position;
g_kinect_controller.getDepthGenerator().ConvertRealWorldToProjective(2, pt, pt);
std_msgs::Int32MultiArray r_array;
r_array.data.clear();
r_array.data.push_back(pt[1].X);
r_array.data.push_back(pt[1].Y);
r_hand_pub.publish(r_array);
}
}
}
void glutDisplay (void)
{
xn::SceneMetaData sceneMD;
xn::DepthMetaData depthMD;
g_kinect_controller.getContext().WaitAndUpdateAll();
g_skeleton_tracker.processKinect(g_kinect_controller);
g_kinect_controller.getDepthGenerator().GetMetaData(depthMD);
g_kinect_controller.getUserGenerator().GetUserPixels(0, sceneMD);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup the OpenGL viewpoint
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glLoadIdentity();
glOrtho(0, depthMD.XRes(), depthMD.YRes(), 0, -1.0, 1.0);
glDisable(GL_TEXTURE_2D);
kinect_display_drawDepthMapGL(depthMD, sceneMD);
kinect_display_drawSkeletonGL(g_kinect_controller.getUserGenerator(),
g_kinect_controller.getDepthGenerator());
/* ADDED */
char strLabel[50] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
(g_kinect_controller.getUserGenerator()).GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
if ((g_kinect_controller.getUserGenerator()).GetSkeletonCap().IsTracking(aUsers[i]))
{
// LEFT
XnSkeletonJointPosition joint1, joint2;
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_ELBOW, joint1);
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_LEFT_HAND, joint2);
if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
{
return;
}
XnPoint3D pt[2];
pt[0] = joint1.position;
pt[1] = joint2.position;
g_kinect_controller.getDepthGenerator().ConvertRealWorldToProjective(2, pt, pt);
std_msgs::Int32MultiArray l_array;
l_array.data.clear();
l_array.data.push_back(pt[1].X);
l_array.data.push_back(pt[1].Y);
l_hand_pub.publish(l_array);
// RIGHT
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_ELBOW, joint1);
(g_kinect_controller.getUserGenerator()).GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_RIGHT_HAND, joint2);
if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
{
return;
}
pt[0] = joint1.position;
pt[1] = joint2.position;
g_kinect_controller.getDepthGenerator().ConvertRealWorldToProjective(2, pt, pt);
std_msgs::Int32MultiArray r_array;
r_array.data.clear();
r_array.data.push_back(pt[1].X);
r_array.data.push_back(pt[1].Y);
r_hand_pub.publish(r_array);
}
}
glutSwapBuffers();
}
