diff --git a/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h b/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h index a0ed6fa1..48e257ae 100644 --- a/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h +++ b/tool_path_planner/include/tool_path_planner/plane_slicer_raster_generator.h @@ -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 { @@ -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; } @@ -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; } @@ -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(); } }; diff --git a/tool_path_planner/src/plane_slicer_raster_generator.cpp b/tool_path_planner/src/plane_slicer_raster_generator.cpp index 295874d3..c9515806 100644 --- a/tool_path_planner/src/plane_slicer_raster_generator.cpp +++ b/tool_path_planner/src/plane_slicer_raster_generator.cpp @@ -461,9 +461,12 @@ boost::optional PlaneSlicerRasterGenerator::generate() VectorXd bounds(6); std::vector 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; @@ -485,7 +488,7 @@ boost::optional PlaneSlicerRasterGenerator::generate() { // computing major axes using oob and assign to x_dir, y_dir, z_dir vtkSmartPointer oob = vtkSmartPointer::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 diff --git a/tool_path_planner/src/utilities.cpp b/tool_path_planner/src/utilities.cpp index e01e1889..e5a74ae1 100644 --- a/tool_path_planner/src/utilities.cpp +++ b/tool_path_planner/src/utilities.cpp @@ -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;