OpenRAVE::InterfaceBasePtr CreateInterfaceValidated(OpenRAVE::InterfaceType type,
std::string const &interface_name, std::istream &sinput, OpenRAVE::EnvironmentBasePtr env)
{
if (type == OpenRAVE::PT_Sensor && interface_name == "bhtactilesensor") {
std::string node_name, owd_namespace, robot_name, link_prefix;
sinput >> node_name >> owd_namespace >> robot_name >> link_prefix;
// Initialize the ROS node.
RAVELOG_DEBUG("name = %s namespace = %s\n", node_name.c_str(), owd_namespace.c_str());
if (sinput.fail()) {
RAVELOG_ERROR("BHTactileSensor is missing the node_name, owd_namespace, robot_name, or link_prefix parameter(s).\n");
return OpenRAVE::InterfaceBasePtr();
}
if (!ros::isInitialized()) {
int argc = 0;
ros::init(argc, NULL, node_name, ros::init_options::AnonymousName);
RAVELOG_DEBUG("Starting ROS node '%s'.\n", node_name.c_str());
} else {
RAVELOG_DEBUG("Using existing ROS node '%s'\n", ros::this_node::getName().c_str());
}
OpenRAVE::RobotBasePtr robot = env->GetRobot(robot_name);
if (!robot) {
throw OPENRAVE_EXCEPTION_FORMAT("There is no robot named '%s'.",
robot_name.c_str(), OpenRAVE::ORE_InvalidArguments);
}
return boost::make_shared<BHTactileSensor>(env, robot, owd_namespace, link_prefix);
} else {
void ReadFile(RobotBasePtr& probot, const std::string& filename, const AttributesList& atts)
{
std::ifstream f(filename.c_str());
if( !f ) {
throw OPENRAVE_EXCEPTION_FORMAT("failed to read %s filename",filename,ORE_InvalidArguments);
}
f.seekg(0,ios::end);
std::vector<char> filedata(static_cast<size_t>(f.tellg())+1, 0); // need a null-terminator
f.seekg(0,ios::beg);
f.read(&filedata[0], filedata.size());
Read(probot,filedata,atts);
probot->__struri = filename;
#if defined(HAVE_BOOST_FILESYSTEM) && BOOST_VERSION >= 103600 // stem() was introduced in 1.36
boost::filesystem::path bfpath(filename);
#if defined(BOOST_FILESYSTEM_VERSION) && BOOST_FILESYSTEM_VERSION >= 3
probot->SetName(utils::ConvertToOpenRAVEName(bfpath.stem().string()));
#else
probot->SetName(utils::ConvertToOpenRAVEName(bfpath.stem()));
#endif
#endif
}
