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_); }
void ToasterSimuRobotReader::robotJointStateCallBack(const toaster_msgs::RobotListStamped::ConstPtr& msg) { //std::cout << "[area_manager][DEBUG] new data for robot received" << std::endl; Robot* curRobot; double roll, pitch, yaw; for (unsigned int i = 0; i < msg->robotList.size(); i++) { // If this robot is not assigned we have to allocate data. if (lastConfig_.find(msg->robotList[i].meAgent.meEntity.id) == lastConfig_.end()) { curRobot = new Robot(msg->robotList[i].meAgent.meEntity.id); } else curRobot = lastConfig_[msg->robotList[i].meAgent.meEntity.id]; std::vector<double> robOrientation; double roll, pitch, ywa; bg::model::point<double, 3, bg::cs::cartesian> robPosition; Mobility curRobMobility = FULL; curRobot->setId(msg->robotList[i].meAgent.meEntity.id); curRobot->setName(msg->robotList[i].meAgent.meEntity.name); curRobot->setMobility(curRobMobility); curRobot->setTime(msg->robotList[i].meAgent.meEntity.time); curRobot->busyHands_ = msg->robotList[i].meAgent.busyHands; robPosition.set<0>(msg->robotList[i].meAgent.meEntity.pose.position.x); robPosition.set<1>(msg->robotList[i].meAgent.meEntity.pose.position.y); robPosition.set<2>(msg->robotList[i].meAgent.meEntity.pose.position.z); curRobot->setPosition(robPosition); tf::Quaternion q; tf::quaternionMsgToTF(msg->robotList[i].meAgent.meEntity.pose.orientation, q); tf::Matrix3x3(q).getRPY(roll, pitch, yaw); robOrientation.push_back(roll); robOrientation.push_back(pitch); robOrientation.push_back(yaw); curRobot->setOrientation(robOrientation); lastConfig_[curRobot->getId()] = curRobot; //TODO: fullRobot case if (msg->robotList[i].meAgent.skeletonJoint.size() > 0) { Joint * curJnt; for (unsigned int i_jnt = 0; i_jnt < msg->robotList[i].meAgent.skeletonJoint.size(); i_jnt++) { // If this joint is not assigned we have to allocate data. if (lastConfig_[curRobot->getId()]->skeleton_[msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name ] == NULL) { curJnt = new Joint(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->robotList[i].meAgent.meEntity.id); } else curJnt = lastConfig_[curRobot->getId()]->skeleton_[msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name ]; std::vector<double> jointOrientation; bg::model::point<double, 3, bg::cs::cartesian> jointPosition; curJnt->setName(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.name); curJnt->setAgentId(curRobot->getId()); curJnt->setTime(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.time); jointPosition.set<0>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.x); jointPosition.set<1>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.y); jointPosition.set<2>(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.position.z); curJnt->setPosition(jointPosition); tf::Quaternion q; tf::quaternionMsgToTF(msg->robotList[i].meAgent.skeletonJoint[i_jnt].meEntity.pose.orientation, q); tf::Matrix3x3(q).getRPY(roll, pitch, yaw); jointOrientation.push_back(roll); jointOrientation.push_back(pitch); jointOrientation.push_back(yaw); curJnt->setOrientation(jointOrientation); curJnt->position = msg->robotList[i].meAgent.skeletonJoint[i_jnt].position; lastConfig_[curRobot->getId()]->skeleton_[curJnt->getName()] = curJnt; } } } }
void ToasterHumanReader::humanJointStateCallBack(const toaster_msgs::HumanList::ConstPtr& msg) { //std::cout << "[area_manager][DEBUG] new data for human received with time " << msg->humanList[0].meAgent.meEntity.time << std::endl; Human * curHuman; for (unsigned int i = 0; i < msg->humanList.size(); i++) { // If this human is not assigned we have to allocate data. if (lastConfig_.find(msg->humanList[i].meAgent.meEntity.id) == lastConfig_.end()) { curHuman = new Human(msg->humanList[i].meAgent.meEntity.id); } else curHuman = lastConfig_[msg->humanList[i].meAgent.meEntity.id]; std::vector<double> humanOrientation; bg::model::point<double, 3, bg::cs::cartesian> humanPosition; Mobility curHumanMobility = FULL; curHuman->setId(msg->humanList[i].meAgent.meEntity.id); curHuman->setName(msg->humanList[i].meAgent.meEntity.name); curHuman->setMobility(curHumanMobility); curHuman->setTime(msg->humanList[i].meAgent.meEntity.time); curHuman->busyHands_ = msg->humanList[i].meAgent.busyHands; humanPosition.set<0>(msg->humanList[i].meAgent.meEntity.positionX); humanPosition.set<1>(msg->humanList[i].meAgent.meEntity.positionY); humanPosition.set<2>(msg->humanList[i].meAgent.meEntity.positionZ); curHuman->setPosition(humanPosition); humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationRoll); humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationPitch); humanOrientation.push_back(msg->humanList[i].meAgent.meEntity.orientationYaw); curHuman->setOrientation(humanOrientation); lastConfig_[curHuman->getId()] = curHuman; //TODO: fullHuman if (fullHuman_) { Joint * curJnt; for (unsigned int i_jnt = 0; i_jnt < msg->humanList[i].meAgent.skeletonNames.size(); i_jnt++) { // If this joint is not assigned we have to allocate data. if (lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ] == NULL) { curJnt = new Joint(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.id, msg->humanList[i].meAgent.meEntity.id); } else curJnt = lastConfig_[curHuman->getId()]->skeleton_[msg->humanList[i].meAgent.skeletonNames[i_jnt] ]; std::vector<double> jointOrientation; bg::model::point<double, 3, bg::cs::cartesian> jointPosition; curJnt->setName(msg->humanList[i].meAgent.skeletonNames[i_jnt]); curJnt->setAgentId(curHuman->getId()); curJnt->setTime(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.time); jointPosition.set<0>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionX); jointPosition.set<1>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionY); jointPosition.set<2>(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.positionZ); curJnt->setPosition(jointPosition); jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationRoll); jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationPitch); jointOrientation.push_back(msg->humanList[i].meAgent.skeletonJoint[i_jnt].meEntity.orientationYaw); curJnt->setOrientation(jointOrientation); lastConfig_[curHuman->getId()]->skeleton_[curJnt->getName()] = curJnt; } } } }