/**
* Load the vertex and normal information from a obj file
* - Clears current information from model
* - Currently only supports objs that have one mesh and ignore material of mesh (must be set up somewhere else
* - Currently loads face based normals
* @param filename (name of obj file)
* @param Model (model to load information);
* @return bool (if file exists)
**/
bool init_model_from_obj(const wchar_t *filename, Model *model){
// Is the model init'd?
if (model == NULL)
return false;
std::ifstream obj_file(filename);
//Check if file exists
if (!obj_file) {
return false;
}
std::vector<Vector2f> vertex_texture;
std::vector<Vector4f> vertex_normal;
float x, y, z, w;
int a, b, c, d, e, f, g, h, i;
std::string line;
while (getline(obj_file, line)){
// Found new vertex
if (line[0] == 'v' && line[1] == ' '){
sscanf_s(&line[0], "v %f %f %f", &x, &y, &z);
model->verts.push_back({ x, y, z * -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f });
}
// Found new texture mapping coord
else if (line[0] == 'v' && line[1] == 't'){
sscanf_s(&line[0], "vt %f %f", &x, &y);
vertex_texture.push_back(Vector2f( x, 1.0f - y ));
}
// Found new normal
else if (line[0] == 'v' && line[1] == 'n'){
sscanf_s(&line[0], "vn %f %f %f %f", &x, &y, &z, &w);
vertex_normal.push_back(Vector4f( x, y, z * -1.0f, w ));
}
// Found new face
else if (line[0] == 'f'){
sscanf_s(&line[0], "f %d/%d/%d %d/%d/%d %d/%d/%d", &a, &d, &g, &b, &e, &h, &c, &f, &i);
model->indices.push_back((a - 1));
model->verts[(a - 1)].tU = vertex_texture[(d - 1)](0);
model->verts[(a - 1)].tV = vertex_texture[(d - 1)](1);
model->verts[(a - 1)].nX = vertex_normal[(g - 1)](0);
model->verts[(a - 1)].nY = vertex_normal[(g - 1)](1);
model->verts[(a - 1)].nZ = vertex_normal[(g - 1)](2);
model->verts[(a - 1)].nW = vertex_normal[(g - 1)](3);
model->indices.push_back((c - 1));
model->verts[(c - 1)].tU = vertex_texture[(f - 1)](0);
model->verts[(c - 1)].tV = vertex_texture[(f - 1)](1);
model->verts[(c - 1)].nX = vertex_normal[(i - 1)](0);
model->verts[(c - 1)].nY = vertex_normal[(i - 1)](1);
model->verts[(c - 1)].nZ = vertex_normal[(i - 1)](2);
model->verts[(c - 1)].nW = vertex_normal[(i - 1)](3);
model->indices.push_back((b - 1));
model->verts[(b - 1)].tU = vertex_texture[(e - 1)](0);
model->verts[(b - 1)].tV = vertex_texture[(e - 1)](1);
model->verts[(b - 1)].nX = vertex_normal[(h - 1)](0);
model->verts[(b - 1)].nY = vertex_normal[(h - 1)](1);
model->verts[(b - 1)].nZ = vertex_normal[(h - 1)](2);
model->verts[(b - 1)].nW = vertex_normal[(h - 1)](3);
}
}
return true;
}
void VelocityCurtainNodelet::onInit() {
ros::NodeHandle node(getNodeHandle());
ros::NodeHandle private_node(getPrivateNodeHandle());
double width(0.5);
private_node.param("robot_width", width, 0.5);
double depth(0.5);
private_node.param("robot_depth", depth, 0.5);
double height(1.5);
private_node.param("robot_height", height, 1.5);
double keep_duration(1.0);
private_node.param("keep_duration", keep_duration, 1.0);
std::string base_frame("base_link");
private_node.param<std::string>("base_frame_id", base_frame, "base_link");
using Eigen::Vector4f;
robot_ = boost::shared_ptr<BoxRobotBody>(
new BoxRobotBody(Vector4f(-depth/2, -width/2, 0.0, 0.0),
Vector4f(depth/2, width/2, height, 0.0)));
filter_ = boost::shared_ptr<ForwardVelocityFilter>(
new ForwardVelocityFilter(node,
"input_velocity",
"output_velocity",
10));
curtain_ = boost::shared_ptr<LightCurtain>(
new LightCurtain(
boost::bind(&BoxRobotBody::isCollided, robot_, _1),
boost::bind(&ForwardVelocityFilter::set_danger, filter_, _1),
keep_duration));
pointcloud_updater_ = boost::shared_ptr<PointCloudROS>(
new PointCloudROS(
node,
"/curtain/points",
base_frame,
boost::bind(&LightCurtain::updatePointCloud, curtain_, _1)));
laser_updater_ = boost::shared_ptr<LaserROS>(
new LaserROS(
node,
"/curtain/scan",
base_frame,
boost::bind(&LightCurtain::updatePointCloud, curtain_, _1)));
// this calls callback now, is this bug?
config_server_ = boost::shared_ptr<ConfigServer>(new ConfigServer);
config_server_->setCallback(boost::bind(&VelocityCurtainNodelet::reconfigureCallback, this, _1, _2));
// set again because above bug reset the params
robot_->setMinPoint(Vector4f(-depth/2,
-width/2,
0.0,
0.0));
robot_->setMaxPoint(Vector4f(depth/2,
width/2,
height,
0.0));
curtain_->setKeepDuration(keep_duration);
pointcloud_updater_->setBaseFrameId(base_frame);
laser_updater_->setBaseFrameId(base_frame);
pointcloud_updater_->init();
laser_updater_->init();
curtain_->init();
}
