bool WorldRosItem::spawnModel(gazebo_msgs::SpawnModel::Request &req,
gazebo_msgs::SpawnModel::Response &res)
{
std::string model_name = req.model_name;
std::string model_xml = req.model_xml;
cnoid::Vector3 trans;
cnoid::Quat R;
trans(0) = req.initial_pose.position.x;
trans(1) = req.initial_pose.position.y;
trans(2) = req.initial_pose.position.z;
R.w() = req.initial_pose.orientation.w;
R.x() = req.initial_pose.orientation.x;
R.y() = req.initial_pose.orientation.y;
R.z() = req.initial_pose.orientation.z;
BodyItemPtr body = new BodyItem();
body->setName(req.model_name);
const char *fname = tmpnam(NULL);
std::ofstream ofs(fname);
ofs << model_xml << std::endl;
ofs.close();
body->loadModelFile(fname);
remove(fname);
body->body()->rootLink()->setTranslation(trans);
body->body()->rootLink()->setRotation(R.matrix());
world->addChildItem(body);
BodyRosItemPtr bodyros = new BodyRosItem();
bodyros->setName(req.model_name + "_ROS");
body->addChildItem(bodyros);
ItemTreeView::instance()->checkItem(body);
ItemTreeView::instance()->checkItem(bodyros);
return true;
}
