Skip to content

Commit

Permalink
Clang fixes and PlaneSlicerConfig Serialization
Browse files Browse the repository at this point in the history
  • Loading branch information
@DavidMerzJr
DavidMerzJr committed Dec 14, 2020
1 parent 12cf52c commit 5905069
Show file tree
Hide file tree
Showing 3 changed files with 46 additions and 14 deletions.
35 changes: 33 additions & 2 deletions tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,8 @@ class PlaneSlicerRasterGenerator : public PathGenerator
double min_segment_size{ DEFAULT_MIN_SEGMENT_SIZE };
double search_radius{ DEFAULT_SEARCH_RADIUS };
double min_hole_size{ DEFAULT_MIN_HOLE_SIZE };
bool raster_wrt_global_axes { DEFAULT_RASTER_WRT_GLOBAL_AXES };
Eigen::Vector3d raster_direction { Eigen::Vector3d::UnitY() };
bool raster_wrt_global_axes{ DEFAULT_RASTER_WRT_GLOBAL_AXES };
Eigen::Vector3d raster_direction{ Eigen::Vector3d::UnitY() };

Json::Value toJson() const
{
Expand All @@ -69,6 +69,12 @@ class PlaneSlicerRasterGenerator : public PathGenerator
jv["min_segment_size"] = min_segment_size;
jv["search_radius"] = search_radius;
jv["min_hole_size"] = min_hole_size;
jv["raster_wrt_global_axes"] = raster_wrt_global_axes;
Json::Value raster_dir(Json::ValueType::objectValue);
raster_dir["x"] = raster_direction.x();
raster_dir["y"] = raster_direction.y();
raster_dir["z"] = raster_direction.z();
jv["raster_direction"] = raster_dir;

return jv;
}
Expand Down Expand Up @@ -104,6 +110,26 @@ class PlaneSlicerRasterGenerator : public PathGenerator
DEFAULT_SEARCH_RADIUS;
min_hole_size = validate(jv, "min_hole_size", Json::ValueType::realValue) ? jv["min_hole_size"].asDouble() :
DEFAULT_MIN_HOLE_SIZE;
raster_wrt_global_axes = validate(jv, "raster_wrt_global_axes", Json::ValueType::booleanValue) ?
jv["raster_wrt_global_axes"].asBool() :
DEFAULT_RASTER_WRT_GLOBAL_AXES;
if (jv["raster_direction"].isNull() || jv["raster_direction"].type() != Json::ValueType::objectValue)
{
ROS_ERROR("Malformed Raster Direction in Json value");
return false;
}
if (validate(jv["raster_direction"], "x", Json::ValueType::objectValue) &&
validate(jv["raster_direction"], "y", Json::ValueType::objectValue) &&
validate(jv["raster_direction"], "z", Json::ValueType::objectValue))
{
raster_direction.x() = jv["raster_direction"]["x"].asDouble();
raster_direction.y() = jv["raster_direction"]["y"].asDouble();
raster_direction.z() = jv["raster_direction"]["z"].asDouble();
}
else
{
raster_direction = Eigen::Vector3d::UnitY();
}
return true;
}

Expand All @@ -125,6 +151,11 @@ class PlaneSlicerRasterGenerator : public PathGenerator
ss << "raster_rot_offset: " << raster_rot_offset << std::endl;
ss << "min_segment_size: " << min_segment_size << std::endl;
ss << "search_radius: " << search_radius << std::endl;
ss << "raster_wrt_global_axes: " << raster_wrt_global_axes << std::endl;
ss << "raster_direction: " << std::endl;
ss << " x: " << raster_direction.x() << std::endl;
ss << " y: " << raster_direction.y() << std::endl;
ss << " z: " << raster_direction.z() << std::endl;
return ss.str();
}
};
Expand Down
11 changes: 7 additions & 4 deletions tool_path_planner/src/plane_slicer_raster_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -461,9 +461,12 @@ boost::optional<ToolPaths> PlaneSlicerRasterGenerator::generate()
VectorXd bounds(6);
std::vector<Vector3d> extents;
mesh_data_->GetBounds(bounds.data());
extents.push_back(Vector3d::UnitX() * (bounds[1] - bounds[0])); // Extent in x-direction of supplied mesh coordinate frame
extents.push_back(Vector3d::UnitY() * (bounds[3] - bounds[2])); // Extent in y-direction of supplied mesh coordinate frame
extents.push_back(Vector3d::UnitZ() * (bounds[5] - bounds[4])); // Extent in z-direction of supplied mesh coordinate frame
extents.push_back(Vector3d::UnitX() * (bounds[1] - bounds[0])); // Extent in x-direction of supplied mesh
// coordinate frame
extents.push_back(Vector3d::UnitY() * (bounds[3] - bounds[2])); // Extent in y-direction of supplied mesh
// coordinate frame
extents.push_back(Vector3d::UnitZ() * (bounds[5] - bounds[4])); // Extent in z-direction of supplied mesh
// coordinate frame

// find min and max magnitude.
int max = 0;
Expand All @@ -485,7 +488,7 @@ boost::optional<ToolPaths> PlaneSlicerRasterGenerator::generate()
{
// computing major axes using oob and assign to x_dir, y_dir, z_dir
vtkSmartPointer<vtkOBBTree> oob = vtkSmartPointer<vtkOBBTree>::New();
oob->ComputeOBB(mesh_data_,corner.data(), x_dir.data(), y_dir.data(), z_dir.data(), sizes.data());
oob->ComputeOBB(mesh_data_, corner.data(), x_dir.data(), y_dir.data(), z_dir.data(), sizes.data());
}

// Compute the center of mass
Expand Down
14 changes: 6 additions & 8 deletions tool_path_planner/src/utilities.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -235,15 +235,13 @@ bool toPlaneSlicerConfig(PlaneSlicerRasterGenerator::Config& config,
config.raster_wrt_global_axes = config_msg.raster_wrt_global_axes;

// Check that the raster direction was set; we are not interested in direction [0,0,0]
double norm_squared =
config_msg.raster_direction.x * config_msg.raster_direction.x +
config_msg.raster_direction.y * config_msg.raster_direction.y +
config_msg.raster_direction.z * config_msg.raster_direction.z;
if (norm_squared > 0.000001)
Eigen::Vector3d test_raster_direction;
test_raster_direction.x() = config_msg.raster_direction.x;
test_raster_direction.y() = config_msg.raster_direction.y;
test_raster_direction.z() = config_msg.raster_direction.z;
if (!test_raster_direction.isApprox(Eigen::Vector3d::Zero()))
{
config.raster_direction.x() = config_msg.raster_direction.x;
config.raster_direction.y() = config_msg.raster_direction.y;
config.raster_direction.z() = config_msg.raster_direction.z;
config.raster_direction = test_raster_direction;
}

return true;
Expand Down

0 comments on commit 5905069

Please sign in to comment.