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