本文整理汇总了C++中MovableObject::setPosition方法的典型用法代码示例。如果您正苦于以下问题：C++ MovableObject::setPosition方法的具体用法？C++ MovableObject::setPosition怎么用？C++ MovableObject::setPosition使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类MovableObject的用法示例。
在下文中一共展示了MovableObject::setPosition方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: CallbackObj
void GazeboObjectReader::CallbackObj(const gazebo_msgs::ModelStates::ConstPtr& msg) {
ros::Time now = ros::Time::now();
MovableObject* curObject;
std::vector<std::string> objectsName = msg->name;
std::vector<geometry_msgs::Pose> objectsPose = msg->pose;
if(objectsName.size() != objectsPose.size()){
ROS_ERROR("[GazeboObjectReader] Topic msg invalid: nb of objects diff from nb of poses!");
return;
}
for (int i = 0; i < objectsName.size(); i++) {
if(objectsName[i] == "pr2" || objectsName[i] == "ground_plane"){
//we ignore these objects
continue;
}
// If this object is not assigned we have to allocate data.
if (lastConfig_.find(objectsName[i]) == lastConfig_.end()) {
curObject = new MovableObject(objectsName[i]);
curObject->setRoomId(0);
curObject->setName(objectsName[i]);
} else{
curObject = lastConfig_[objectsName[i]];
}
std::vector<double> objOrientation;
bg::model::point<double, 3, bg::cs::cartesian> objPosition;
curObject->setId(objectsName[i]);
curObject->setTime(now.toNSec());
objPosition.set<0>(objectsPose[i].position.x);
objPosition.set<1>(objectsPose[i].position.y);
objPosition.set<2>(objectsPose[i].position.z);
curObject->setPosition(objPosition);
tf::Quaternion q;
double roll, pitch, yaw;
tf::quaternionMsgToTF(objectsPose[i].orientation, q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
objOrientation.push_back(roll);
objOrientation.push_back(pitch);
objOrientation.push_back(yaw);
curObject->setOrientation(objOrientation);
lastConfig_[objectsName[i]] = curObject;
}
}