diff --git a/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h b/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h index 0c9f420f..5918847d 100644 --- a/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h +++ b/include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h @@ -31,7 +31,10 @@ namespace owds { void addObject(const std::string& name, const std::array& translation, - const std::array& rotation); + const std::array& rotation, + const std::array& scale); + + bool readConfiguration(std::string path); }; } // namespace owds diff --git a/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp b/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp index cfad688f..fa44930f 100644 --- a/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp +++ b/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp @@ -33,60 +33,8 @@ namespace owds { onto_ = ontologies_manipulator_->get(robot_name); onto_->close(); - if(config_file_ != "") - { - std::string package_pattern = "package://"; - if(config_file_.find(package_pattern) != std::string::npos) - { - size_t pose = config_file_.find(package_pattern); - size_t pose_end_of_name = config_file_.find("/", pose + package_pattern.size()); - std::string full_package = config_file_.substr(pose, pose_end_of_name - pose); - std::string package_name = config_file_.substr(pose + package_pattern.size(), pose_end_of_name - pose - package_pattern.size()); - std::string package_path = ros::package::getPath(package_name); - config_file_.replace(config_file_.find(full_package), full_package.size(), package_path); - } - - YamlReader reader; - if(reader.read(config_file_)) - { - auto objects_ids = reader.getKeys(); - for(auto& id : objects_ids) - { - double pose_x = 0, pose_y = 0, pose_z = 0; - double orientation_x = 0, orientation_y = 0, orientation_z = 0; - auto obj_description = reader[id]; - - if(obj_description.keyExists("position")) - { - auto position = obj_description["position"]; - if(position.keyExists("x")) - pose_x = std::stod(position["x"].value().front()); - if(position.keyExists("y")) - pose_y = std::stod(position["y"].value().front()); - if(position.keyExists("z")) - pose_z = std::stod(position["z"].value().front()); - } - - if(obj_description.keyExists("orientation")) - { - auto orientation = obj_description["orientation"]; - if(orientation.keyExists("x")) - orientation_x = std::stod(orientation["x"].value().front()); - if(orientation.keyExists("y")) - orientation_y = std::stod(orientation["y"].value().front()); - if(orientation.keyExists("z")) - orientation_z = std::stod(orientation["z"].value().front()); - } - - addObject(id, {pose_x, pose_y, pose_z}, {orientation_x, orientation_y, orientation_z}); - } - } - else - { - ShellDisplay::error("[StaticObjectsPerceptionModule] fail to read file \'" + config_file_ + "\'"); - return false; - } - } + if(config_file_.empty() == false) + return readConfiguration(config_file_); return true; } @@ -120,7 +68,8 @@ namespace owds { void StaticObjectsPerceptionModule::addObject(const std::string& name, const std::array& translation, - const std::array& rotation) + const std::array& rotation, + const std::array& scale) { if(onto_ == nullptr) { @@ -132,6 +81,7 @@ namespace owds { Shape_t shape = ontology::getEntityShape(onto_, name); if(shape.type == SHAPE_MESH) { + shape.scale = scale; obj.setShape(shape); Pose pose(translation, rotation); obj.updatePose(pose); @@ -146,6 +96,80 @@ namespace owds { ShellDisplay::warning("[StaticObjectsPerceptionModule] No mesh defined in the ontology for entity \'" + name + "\'"); } + bool StaticObjectsPerceptionModule::readConfiguration(std::string path) +{ + std::string package_pattern = "package://"; + if(path.find(package_pattern) != std::string::npos) + { + size_t pose = path.find(package_pattern); + size_t pose_end_of_name = path.find("/", pose + package_pattern.size()); + std::string full_package = path.substr(pose, pose_end_of_name - pose); + std::string package_name = path.substr(pose + package_pattern.size(), pose_end_of_name - pose - package_pattern.size()); + std::string package_path = ros::package::getPath(package_name); + path.replace(path.find(full_package), full_package.size(), package_path); + } + + YamlReader reader; + if(reader.read(path)) + { + bool success=true; + auto objects_ids = reader.getKeys(); + + if(reader.keyExists("include")) + { + auto paths = reader["include"].value(); + for(const auto& path : paths) + { + if(readConfiguration(path) == false) + success = false; + } + } + + for(auto& id : objects_ids) + { + if(id == "include") + continue; + + double pose_x = 0, pose_y = 0, pose_z = 0; + double orientation_x = 0, orientation_y = 0, orientation_z = 0; + double scale_x=1,scale_y=1,scale_z=1; + auto obj_description = reader[id]; + + if(obj_description.keyExists("position")) + { + auto position = obj_description["position"]; + if(position.keyExists("x")) pose_x = std::stod(position["x"].value().front()); + if(position.keyExists("y")) pose_y = std::stod(position["y"].value().front()); + if(position.keyExists("z")) pose_z = std::stod(position["z"].value().front()); + } + + if(obj_description.keyExists("orientation")) + { + auto orientation = obj_description["orientation"]; + if(orientation.keyExists("x")) orientation_x = std::stod(orientation["x"].value().front()); + if(orientation.keyExists("y")) orientation_y = std::stod(orientation["y"].value().front()); + if(orientation.keyExists("z")) orientation_z = std::stod(orientation["z"].value().front()); + } + + if(obj_description.keyExists("scale")) + { + auto scale = obj_description["scale"]; + if(scale.keyExists("x")) scale_x = std::stod(scale["x"].value().front()); + if(scale.keyExists("y")) scale_y = std::stod(scale["y"].value().front()); + if(scale.keyExists("z")) scale_z = std::stod(scale["z"].value().front()); + } + + addObject(id, {pose_x, pose_y, pose_z}, {orientation_x, orientation_y, orientation_z},{scale_x,scale_y,scale_z}); + } + return success; + } + else + { + ShellDisplay::error("[StaticObjectsPerceptionModule] fail to read file \'" + path + "\'"); + return false; + } +} + } // namespace owds PLUGINLIB_EXPORT_CLASS(owds::StaticObjectsPerceptionModule, owds::PerceptionModuleBase_) \ No newline at end of file