Skip to content

Commit

Permalink
[StaticObjectsPerceptionModule] allow include key in configuration file
Browse files Browse the repository at this point in the history
  • Loading branch information
@sarthou
sarthou committed Dec 19, 2024
1 parent 2b1c11e commit 4574a17
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 56 deletions.
5 changes: 4 additions & 1 deletion include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,10 @@ namespace owds {

void addObject(const std::string& name,
const std::array<double, 3>& translation,
const std::array<double, 3>& rotation);
const std::array<double, 3>& rotation,
const std::array<double,3>& scale);

bool readConfiguration(std::string path);
};

} // namespace owds
Expand Down
134 changes: 79 additions & 55 deletions src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -120,7 +68,8 @@ namespace owds {

void StaticObjectsPerceptionModule::addObject(const std::string& name,
const std::array<double, 3>& translation,
const std::array<double, 3>& rotation)
const std::array<double, 3>& rotation,
const std::array<double,3>& scale)
{
if(onto_ == nullptr)
{
Expand All @@ -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);
Expand All @@ -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_<owds::Object>)

0 comments on commit 4574a17

Please sign in to comment.