Skip to content

Commit b36f715

Browse files
amockamock
authored
Merge pull request #70 from naturerobots/lvr-update
LVR Update
2 parents 5007b3d + 3f2e411 commit b36f715

29 files changed

+352
-825
lines changed

‎cvp_mesh_planner/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,13 @@
11
cmake_minimum_required(VERSION 3.8)
22
project(cvp_mesh_planner)
33

4+
# DEFAULT RELEASE
5+
if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
6+
if (NOT CMAKE_BUILD_TYPE)
7+
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
8+
endif()
9+
endif()
10+
411
find_package(ament_cmake_ros REQUIRED)
512
find_package(mbf_mesh_core REQUIRED)
613
find_package(mbf_msgs REQUIRED)

‎cvp_mesh_planner/src/cvp_mesh_planner.cpp‎

Lines changed: 50 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -65,18 +65,20 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start,
6565
std::vector<geometry_msgs::msg::PoseStamped>& plan, double& cost,
6666
std::string& message)
6767
{
68-
const auto& mesh = mesh_map_->mesh();
68+
const auto mesh = mesh_map_->mesh();
6969
std::list<std::pair<mesh_map::Vector, lvr2::FaceHandle>> path;
7070

7171
// mesh_map->combineVertexCosts(); // TODO should be outside the planner
7272

73-
RCLCPP_INFO(node_->get_logger(), "start wave front propagation.");
73+
RCLCPP_DEBUG_STREAM(node_->get_logger(), "start wave front propagation.");
7474

7575
mesh_map::Vector goal_vec = mesh_map::toVector(goal.pose.position);
7676
mesh_map::Vector start_vec = mesh_map::toVector(start.pose.position);
7777

7878
const uint32_t outcome = waveFrontPropagation(goal_vec, start_vec, path, message);
7979

80+
RCLCPP_DEBUG_STREAM(node_->get_logger(), "finished wave front propagation.");
81+
8082
path.reverse();
8183

8284
std_msgs::msg::Header header;
@@ -161,8 +163,8 @@ bool CVPMeshPlanner::initialize(const std::string& plugin_name, const std::share
161163
}
162164

163165
path_pub_ = node->create_publisher<nav_msgs::msg::Path>("~/path", rclcpp::QoS(1).transient_local());
164-
const auto& mesh = mesh_map_->mesh();
165-
direction_ = lvr2::DenseVertexMap<float>(mesh.nextVertexIndex(), 0);
166+
const auto mesh = mesh_map_->mesh();
167+
direction_ = lvr2::DenseVertexMap<float>(mesh->nextVertexIndex(), 0);
166168
// TODO check all map dependencies! (loaded layers etc...)
167169

168170
reconfiguration_callback_handle_ = node_->add_on_set_parameters_callback(std::bind(
@@ -189,11 +191,11 @@ rcl_interfaces::msg::SetParametersResult CVPMeshPlanner::reconfigureCallback(std
189191

190192
void CVPMeshPlanner::computeVectorMap()
191193
{
192-
const auto& mesh = mesh_map_->mesh();
194+
const auto mesh = mesh_map_->mesh();
193195
const auto& face_normals = mesh_map_->faceNormals();
194196
const auto& vertex_normals = mesh_map_->vertexNormals();
195197

196-
for (auto v3 : mesh.vertices())
198+
for (auto v3 : mesh->vertices())
197199
{
198200
// if(vertex_costs[v3] > config.cost_limit || !predecessors.containsKey(v3))
199201
// continue;
@@ -212,8 +214,8 @@ void CVPMeshPlanner::computeVectorMap()
212214

213215
const lvr2::FaceHandle& fH = optFh.get();
214216

215-
const auto& vec3 = mesh.getVertexPosition(v3);
216-
const auto& vec1 = mesh.getVertexPosition(v1);
217+
const auto& vec3 = mesh->getVertexPosition(v3);
218+
const auto& vec1 = mesh->getVertexPosition(v1);
217219

218220
// compute the direction vector and rotate it by theta, which is stored in
219221
// the direction vertex map
@@ -237,21 +239,21 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
237239
const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2,
238240
const lvr2::VertexHandle& v3)
239241
{
240-
const auto& mesh = mesh_map_->mesh();
242+
const auto mesh = mesh_map_->mesh();
241243

242244
const double u1 = distances[v1];
243245
const double u2 = distances[v2];
244246
const double u3 = distances[v3];
245247

246-
const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1, v2);
248+
const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1, v2);
247249
const double c = edge_weights[e12h.unwrap()];
248250
const double c_sq = c * c;
249251

250-
const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1, v3);
252+
const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1, v3);
251253
const double b = edge_weights[e13h.unwrap()];
252254
const double b_sq = b * b;
253255

254-
const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2, v3);
256+
const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2, v3);
255257
const double a = edge_weights[e23h.unwrap()];
256258
const double a_sq = a * a;
257259

@@ -284,7 +286,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
284286
predecessors_[v3] = v1;
285287
direction_[v3] = static_cast<float>(theta);
286288
distances[v3] = static_cast<float>(u3tmp);
287-
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
289+
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
288290
cutting_faces_.insert(v3, fh);
289291
#ifdef DEBUG
290292
mesh_map->publishDebugVector(v3, v1, fh, theta, mesh_map::color(0.9, 0.9, 0.2),
@@ -300,7 +302,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
300302
predecessors_[v3] = v1;
301303
direction_[v3] = 0;
302304
distances[v3] = u3tmp;
303-
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
305+
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
304306
cutting_faces_.insert(v3, fh);
305307
#ifdef DEBUG
306308
mesh_map->publishDebugVector(v3, v1, fh, 0, mesh_map::color(0.9, 0.9, 0.2),
@@ -317,7 +319,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
317319
const double t2cos = (a_sq + u3tmp_sq - u2_sq) / (2 * a * u3tmp);
318320
if (S <= 0 && std::fabs(t2cos) <= 1)
319321
{
320-
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
322+
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
321323
const double theta = -acos(t2cos);
322324
direction_[v3] = static_cast<float>(theta);
323325
distances[v3] = static_cast<float>(u3tmp);
@@ -337,7 +339,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
337339
direction_[v3] = 0;
338340
distances[v3] = u3tmp;
339341
predecessors_[v3] = v2;
340-
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
342+
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
341343
cutting_faces_.insert(v3, fh);
342344
#ifdef DEBUG
343345
mesh_map->publishDebugVector(v3, v2, fh, 0, mesh_map::color(0.9, 0.9, 0.2),
@@ -357,21 +359,21 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
357359
const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2,
358360
const lvr2::VertexHandle& v3)
359361
{
360-
const auto& mesh = mesh_map_->mesh();
362+
const auto mesh = mesh_map_->mesh();
361363

362364
const double u1 = distances[v1];
363365
const double u2 = distances[v2];
364366
const double u3 = distances[v3];
365367

366-
const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1, v2);
368+
const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1, v2);
367369
const double c = edge_weights[e12h.unwrap()];
368370
const double c_sq = c * c;
369371

370-
const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1, v3);
372+
const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1, v3);
371373
const double b = edge_weights[e13h.unwrap()];
372374
const double b_sq = b * b;
373375

374-
const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2, v3);
376+
const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2, v3);
375377
const double a = edge_weights[e23h.unwrap()];
376378
const double a_sq = a * a;
377379

@@ -406,7 +408,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
406408
u3tmp = u1 + b;
407409
if (u3tmp < u3)
408410
{
409-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
411+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
410412
cutting_faces_.insert(v3, fH);
411413
predecessors_[v3] = v1;
412414
#ifdef DEBUG
@@ -425,7 +427,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
425427
u3tmp = u2 + a;
426428
if (u3tmp < u3)
427429
{
428-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
430+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
429431
cutting_faces_.insert(v3, fH);
430432
predecessors_[v3] = v2;
431433
#ifdef DEBUG
@@ -478,7 +480,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
478480

479481
if (theta1 < theta0 && theta2 < theta0)
480482
{
481-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
483+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
482484
cutting_faces_.insert(v3, fH);
483485
distances[v3] = static_cast<float>(u3tmp);
484486
if (theta1 < theta2)
@@ -506,7 +508,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
506508
u3tmp = u1 + b;
507509
if (u3tmp < u3)
508510
{
509-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
511+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
510512
cutting_faces_.insert(v3, fH);
511513
predecessors_[v3] = v1;
512514
distances[v3] = static_cast<float>(u3tmp);
@@ -524,7 +526,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
524526
u3tmp = u2 + a;
525527
if (u3tmp < u3)
526528
{
527-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
529+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
528530
cutting_faces_.insert(v3, fH);
529531
predecessors_[v3] = v2;
530532
distances[v3] = static_cast<float>(u3tmp);
@@ -549,7 +551,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
549551
const lvr2::VertexHandle &v2tmp,
550552
const lvr2::VertexHandle &v3)
551553
{
552-
const auto& mesh = mesh_map_->mesh();
554+
const auto mesh = mesh_map_->mesh();
553555

554556
bool v1_smaller = distances[v1tmp] < distances[v2tmp];
555557
const lvr2::VertexHandle v1 = v1_smaller ? v1tmp : v2tmp;
@@ -559,15 +561,15 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
559561
const double u2 = distances[v2];
560562
const double u3 = distances[v3];
561563

562-
const lvr2::OptionalEdgeHandle e12h = mesh.getEdgeBetween(v1, v2);
564+
const lvr2::OptionalEdgeHandle e12h = mesh->getEdgeBetween(v1, v2);
563565
const double c = edge_weights[e12h.unwrap()];
564566
const double c_sq = c * c;
565567

566-
const lvr2::OptionalEdgeHandle e13h = mesh.getEdgeBetween(v1, v3);
568+
const lvr2::OptionalEdgeHandle e13h = mesh->getEdgeBetween(v1, v3);
567569
const double b = edge_weights[e13h.unwrap()];
568570
const double b_sq = b * b;
569571

570-
const lvr2::OptionalEdgeHandle e23h = mesh.getEdgeBetween(v2, v3);
572+
const lvr2::OptionalEdgeHandle e23h = mesh->getEdgeBetween(v2, v3);
571573
const double a = edge_weights[e23h.unwrap()];
572574
const double a_sq = a * a;
573575

@@ -596,7 +598,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
596598
const double u3_tmp = u1 + t;
597599
if(u3_tmp < u3)
598600
{
599-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
601+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
600602
cutting_faces_.insert(v3, fH);
601603
predecessors_[v3] = v1;
602604
distances[v3] = static_cast<float>(u3_tmp);
@@ -612,7 +614,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
612614

613615
if (u1t < u2t) {
614616
if (u1t < u3) {
615-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
617+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
616618
cutting_faces_.insert(v3, fH);
617619
predecessors_[v3] = v1;
618620
distances[v3] = static_cast<float>(u1t);
@@ -621,7 +623,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
621623
}
622624
} else {
623625
if (u2t < u3) {
624-
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
626+
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
625627
cutting_faces_.insert(v3, fH);
626628
predecessors_[v3] = v2;
627629
distances[v3] = static_cast<float>(u2t);
@@ -645,7 +647,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
645647
{
646648
RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init wave front propagation.");
647649

648-
const auto& mesh = mesh_map_->mesh();
650+
const auto mesh = mesh_map_->mesh();
649651
const auto& vertex_costs = mesh_map_->vertexCosts();
650652
auto& invalid = mesh_map_->invalid;
651653

@@ -656,8 +658,8 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
656658
mesh_map::Vector goal = original_goal;
657659

658660
// Find the containing faces of start and goal
659-
const auto& start_opt = mesh_map_->getContainingFace(start, 0.4);
660-
const auto& goal_opt = mesh_map_->getContainingFace(goal, 0.4);
661+
const lvr2::OptionalFaceHandle start_opt = mesh_map_->getContainingFace(start, 0.4);
662+
const lvr2::OptionalFaceHandle goal_opt = mesh_map_->getContainingFace(goal, 0.4);
661663

662664
const auto t_initialization_start = std::chrono::steady_clock::now();
663665

@@ -666,10 +668,12 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
666668

667669
if (!start_opt) {
668670
message = "Could not find a face close enough to the given start pose";
671+
RCLCPP_WARN_STREAM(node_->get_logger(), "waveFrontPropagation(): " << message);
669672
return mbf_msgs::action::GetPath::Result::INVALID_START;
670673
}
671674
if (!goal_opt) {
672675
message = "Could not find a face close enough to the given goal pose";
676+
RCLCPP_WARN_STREAM(node_->get_logger(), "waveFrontPropagation(): " << message);
673677
return mbf_msgs::action::GetPath::Result::INVALID_GOAL;
674678
}
675679

@@ -683,14 +687,15 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
683687
distances.clear();
684688
predecessors.clear();
685689

686-
lvr2::DenseVertexMap<bool> fixed(mesh.nextVertexIndex(), false);
690+
lvr2::DenseVertexMap<bool> fixed(mesh->nextVertexIndex(), false);
687691

688692
// clear vector field map
689693
vector_map_.clear();
690694

695+
RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init distances.");
691696
// initialize distances with infinity
692697
// initialize predecessor of each vertex with itself
693-
for (auto const& vH : mesh.vertices())
698+
for (auto const& vH : mesh->vertices())
694699
{
695700
distances.insert(vH, std::numeric_limits<float>::infinity());
696701
predecessors.insert(vH, vH);
@@ -699,9 +704,9 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
699704
lvr2::Meap<lvr2::VertexHandle, float> pq;
700705
// Set start distance to zero
701706
// add start vertex to priority queue
702-
for (auto vH : mesh.getVerticesOfFace(start_face))
707+
for (auto vH : mesh->getVerticesOfFace(start_face))
703708
{
704-
const mesh_map::Vector diff = start - mesh.getVertexPosition(vH);
709+
const mesh_map::Vector diff = start - mesh->getVertexPosition(vH);
705710
const float dist = diff.length();
706711
distances[vH] = dist;
707712
vector_map_.insert(vH, diff);
@@ -710,13 +715,13 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
710715
pq.insert(vH, dist);
711716
}
712717

713-
std::array<lvr2::VertexHandle, 3> goal_vertices = mesh.getVerticesOfFace(goal_face);
718+
std::array<lvr2::VertexHandle, 3> goal_vertices = mesh->getVerticesOfFace(goal_face);
714719
RCLCPP_DEBUG_STREAM(node_->get_logger(), "The goal is at (" << goal.x << ", " << goal.y << ", " << goal.z << ") at the face ("
715720
<< goal_vertices[0] << ", " << goal_vertices[1] << ", " << goal_vertices[2]
716721
<< ")");
717-
mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1");
718-
mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2");
719-
mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3");
722+
mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1");
723+
mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2");
724+
mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3");
720725

721726
float goal_dist = std::numeric_limits<float>::infinity();
722727

@@ -756,11 +761,11 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
756761
try
757762
{
758763
std::vector<lvr2::FaceHandle> faces;
759-
mesh.getFacesOfVertex(current_vh, faces);
764+
mesh->getFacesOfVertex(current_vh, faces);
760765

761766
for (auto fh : faces)
762767
{
763-
const auto vertices = mesh.getVerticesOfFace(fh);
768+
const auto vertices = mesh->getVerticesOfFace(fh);
764769
const lvr2::VertexHandle& a = vertices[0];
765770
const lvr2::VertexHandle& b = vertices[1];
766771
const lvr2::VertexHandle& c = vertices[2];

‎dijkstra_mesh_planner/CMakeLists.txt‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,13 @@
11
cmake_minimum_required(VERSION 3.8)
22
project(dijkstra_mesh_planner)
33

4+
# DEFAULT RELEASE
5+
if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
6+
if (NOT CMAKE_BUILD_TYPE)
7+
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
8+
endif()
9+
endif()
10+
411
find_package(ament_cmake_ros REQUIRED)
512
find_package(mbf_mesh_core REQUIRED)
613
find_package(mbf_msgs REQUIRED)

0 commit comments

Comments
 (0)