void NiutHumanReader::updateJoint(int i, int j, Joint& curJoint, int toasterId, std::vector<int>& trackedJoints, const niut_msgs::niut_HUMAN_LIST::ConstPtr& msg) {
std::map<int, std::string> niutToString;
niutToString[0] = "HEAD";
niutToString[3] = "TORSO";
niutToString[9] = "L_HAND";
niutToString[15] = "R_HIP";
niutToString[22] = "R_HAND";
double x, y, z;
int niutJoint = trackedJoints[j];
std::string jointString = niutToString[niutJoint];
//Allocate Joint if needed
if (lastConfig_[toasterId]->skeleton_[jointString] == NULL) {
Joint* tmpJoint = new Joint(10001 + j, toasterId);
tmpJoint->setName(jointString);
tmpJoint->setAgentId(toasterId);
lastConfig_[toasterId]->skeleton_[jointString] = tmpJoint;
}
x = msg->filtered_users[i].skeleton.joint[niutJoint].position.x;
y = msg->filtered_users[i].skeleton.joint[niutJoint].position.y;
z = msg->filtered_users[i].skeleton.joint[niutJoint].position.z;
curJoint.position_.set<0>(x);
curJoint.position_.set<1>(y);
curJoint.position_.set<2>(z);
projectJoint(curJoint, kinectPos_);
}
int main(int argc, char** argv){
ros::init(argc, argv, "humanMonitor");
ros::NodeHandle node;
HumanReader humanRd(node);
double far = 2.5;
double close = 1.3;
double distBodies = 0.0;
double distLHandToGripper = 0.0;
double distRHandToGripper = 0.0;
tf::TransformListener listener;
int niut_TORSO = 3;
int niut_LEFT_HAND = 9;
int niut_RIGHT_HAND = 15;
std::string humanTorso = "torso";
std::string humanLHand = "l_hand";
std::string humanRHand = "r_hand";
std::string robotTorso = "torso_lift_link";
std::string robotLGripper = "l_gripper_l_finger_link";
std::string robotRGripper = "r_gripper_l_finger_link";
humanMonitor::niut_JOINT_STR rHandJoint;
humanMonitor::niut_JOINT_STR lHandJoint;
humanMonitor::niut_JOINT_STR torsoJoint;
humanMonitor::niut_JOINT_STR rHandJointW;
humanMonitor::niut_JOINT_STR lHandJointW;
humanMonitor::niut_JOINT_STR torsoJointW;
double kinectPos[6];
//TODO: Move this in a service
kinectPos[0] = 7.25;
kinectPos[1] = 6.55;
kinectPos[2] = 2.2;
kinectPos[3] = 0;
kinectPos[4] = 0.84;
kinectPos[5] = -1.57;
while( node.ok() ){
ros::spinOnce();
// For now we focus on human with trackedId = 0.
if(humanRd.isPresent()){
std::cout << "[Fact] Human is present!" << std::endl;
//Get Human joints in kinect frame
rHandJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_RIGHT_HAND];
lHandJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_LEFT_HAND];
torsoJoint = humanRd.m_LastConfig[0].skeleton.joint[niut_TORSO];
//Get joints in same frame
if(rHandJoint.position.x != 0.0){
projectJoint(rHandJoint, kinectPos, rHandJointW);
m_HumanLastConfig[humanRHand] = rHandJointW;
}
if(lHandJoint.position.x != 0.0){
projectJoint(lHandJoint, kinectPos, lHandJointW);
m_HumanLastConfig[humanLHand] = lHandJointW;
}
if(torsoJoint.position.x != 0.0){
projectJoint(torsoJoint, kinectPos, torsoJointW);
m_HumanLastConfig[humanTorso] = torsoJointW;
}
m_HumanRBuffer.push_back(humanRd.m_LastTime, m_HumanLastConfig);
//Compute human motion:
long timeThreshold = pow(10,9); // 1sec
double distanceThreshold = 0.2; // 10 cms
if( isMoving(m_HumanRBuffer, humanTorso, timeThreshold, distanceThreshold, 2) ){
std::cout << "[Fact] Human is moving!" << std::endl;
}else{
if( isMoving(m_HumanRBuffer, humanRHand, timeThreshold, distanceThreshold*4/3, 3) ){
std::cout << "[Fact] Human right hand is moving" << std::endl;
}
if( isMoving(m_HumanRBuffer, humanLHand, timeThreshold, distanceThreshold*4/3, 3) ){
std::cout << "[Fact] Human left hand is moving" << std::endl;
}
}
updateRobot(listener);
//Compute relative distances (human / robot):
std::cout << "Human Torso x: " << torsoJointW.position.x << "y: " << torsoJointW.position.y << std::endl;
std::cout << "Robot Torso x: " << m_RobotLastConfig[robotTorso].position.x << "y: " << m_RobotLastConfig[robotTorso].position.y << std::endl;
if(torsoJoint.position.x != 0.0){
distBodies = dist2D(torsoJointW, m_RobotLastConfig[robotTorso]);
std::cout << "Dist human robot: " << distBodies << std::endl;
if( distBodies > far ){
std::cout << "[Fact] Human is far" << std::endl;
}else if(distBodies > close){
std::cout << "[Fact] Human is near" << std::endl;
//We compute the hand to gripper distance in NEAR case.
//Left gripper:
distLHandToGripper = dist3D(lHandJointW, m_RobotLastConfig[robotLGripper]);
distRHandToGripper = dist3D(rHandJointW, m_RobotLastConfig[robotLGripper]);
if( (distLHandToGripper < 0.5) || (distRHandToGripper < 0.5)){
std::cout << "[Fact] Danger! Human hand is close to left gripper!" << std::endl;
}
//Right gripper
distLHandToGripper = dist3D(lHandJointW, m_RobotLastConfig[robotRGripper]);
distRHandToGripper = dist3D(rHandJointW, m_RobotLastConfig[robotRGripper]);
if( (distLHandToGripper < 0.5) || (distRHandToGripper < 0.5)){
std::cout << "[Fact] Danger! Human hand is close to right gripper!" << std::endl;
}
}else
std::cout << "[Fact] Human is too close!" << std::endl;
}else
std::cout << "Human Torso not available " << std::endl;
}else
std::cout << "[Fact] Human is not present" << std::endl;
}
}