12
12
// See the License for the specific language governing permissions and
13
13
// limitations under the License.
14
14
15
- #include " ground_segmentation/ransac_ground_filter_nodelet .hpp"
15
+ #include " node .hpp"
16
16
17
17
#include < pcl_ros/transforms.hpp>
18
18
@@ -54,15 +54,6 @@ Eigen::Vector3d getArbitraryOrthogonalVector(const Eigen::Vector3d & input)
54
54
return unit_vec;
55
55
}
56
56
57
- ground_segmentation::PlaneBasis getPlaneBasis (const Eigen::Vector3d & plane_normal)
58
- {
59
- ground_segmentation::PlaneBasis basis;
60
- basis.e_z = plane_normal;
61
- basis.e_x = getArbitraryOrthogonalVector (plane_normal);
62
- basis.e_y = basis.e_x .cross (basis.e_z );
63
- return basis;
64
- }
65
-
66
57
geometry_msgs::msg::Pose getDebugPose (const Eigen::Affine3d & plane_affine)
67
58
{
68
59
geometry_msgs::msg::Pose debug_pose;
@@ -78,8 +69,17 @@ geometry_msgs::msg::Pose getDebugPose(const Eigen::Affine3d & plane_affine)
78
69
}
79
70
} // namespace
80
71
81
- namespace ground_segmentation
72
+ namespace autoware :: ground_segmentation
82
73
{
74
+ PlaneBasis getPlaneBasis (const Eigen::Vector3d & plane_normal)
75
+ {
76
+ PlaneBasis basis;
77
+ basis.e_z = plane_normal;
78
+ basis.e_x = getArbitraryOrthogonalVector (plane_normal);
79
+ basis.e_y = basis.e_x .cross (basis.e_z );
80
+ return basis;
81
+ }
82
+
83
83
using pointcloud_preprocessor::get_param;
84
84
85
85
RANSACGroundFilterComponent::RANSACGroundFilterComponent (const rclcpp::NodeOptions & options)
@@ -203,7 +203,7 @@ Eigen::Affine3d RANSACGroundFilterComponent::getPlaneAffine(
203
203
pcl::PointXYZ centroid_point;
204
204
centroid.get (centroid_point);
205
205
Eigen::Translation<double , 3 > trans (centroid_point.x , centroid_point.y , centroid_point.z );
206
- const ground_segmentation:: PlaneBasis basis = getPlaneBasis (plane_normal);
206
+ const PlaneBasis basis = getPlaneBasis (plane_normal);
207
207
Eigen::Matrix3d rot;
208
208
rot << basis.e_x .x (), basis.e_y .x (), basis.e_z .x (), basis.e_x .y (), basis.e_y .y (), basis.e_z .y (),
209
209
basis.e_x .z (), basis.e_y .z (), basis.e_z .z ();
@@ -396,7 +396,7 @@ rcl_interfaces::msg::SetParametersResult RANSACGroundFilterComponent::paramCallb
396
396
return result;
397
397
}
398
398
399
- } // namespace ground_segmentation
399
+ } // namespace autoware:: ground_segmentation
400
400
401
401
#include < rclcpp_components/register_node_macro.hpp>
402
- RCLCPP_COMPONENTS_REGISTER_NODE (ground_segmentation::RANSACGroundFilterComponent)
402
+ RCLCPP_COMPONENTS_REGISTER_NODE (autoware:: ground_segmentation::RANSACGroundFilterComponent)
0 commit comments