char* MyController::sendSceneInfo(std::string header, int camID) {
m_my->setWheelVelocity(0.0, 0.0);
Vector3d myPos;
m_my->getPosition(myPos);
double x = myPos.x();
double z = myPos.z();
double theta = calcHeadingAngle(); // y方向の回転は無しと考える
// カメラがついているリンク名取得
std::string link = ""; //"WAIST_LINK0"; //m_my->getCameraLinkName(3);
if (camID < 3) {
link = m_my->getCameraLinkName(camID);
} else {
link = "WAIST_LINK0";
}
//const dReal *lpos = m_my->getParts(link.c_str())->getPosition();
Vector3d lpos;
m_my->getParts(link.c_str())->getPosition(lpos);
// カメラの位置取得(リンク座標系)
Vector3d cpos;
m_my->getCamPos(cpos, camID);
// カメラの位置(絶対座標系, ロボットの回転はないものとする)
printf("linkpos: %lf %lf %lf \n", lpos.x(), lpos.y(), lpos.z());
printf("camerapos: %lf %lf %lf \n", cpos.x(), cpos.y(), cpos.z());
Vector3d campos(lpos.x() + cpos.z() * sin(DEG2RAD(theta)),
lpos.y() + cpos.y(),
lpos.z() + cpos.z() * cos(DEG2RAD(theta)));
//Vector3d campos(cpos.x(), cpos.y(), cpos.z());
// カメラの方向取得(ロボットの回転,関節の回転はないものとする)
Vector3d cdir;
m_my->getCamDir(cdir, camID);
char *replyMsg = new char[1024];
sprintf(replyMsg, "%s %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf %6.1lf",
header.c_str(), x, z, theta, campos.x(), campos.y(), campos.z(), cdir.x(), cdir.y(), cdir.z());
printf("%s \n", replyMsg);
m_srv->sendMsgToSrv(replyMsg);
m_my->setWheelVelocity(0.0, 0.0);
return replyMsg;
}
