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; } }