Skip to content

Commit

Permalink
Merge pull request #70 from naturerobots/lvr-update
Browse files Browse the repository at this point in the history 
LVR Update
  • Loading branch information
@amock
amock authored Jan 2, 2025
2 parents 5007b3d + 3f2e411 commit b36f715
Show file tree
Hide file tree
Showing 29 changed files with 352 additions and 825 deletions.
7 changes: 7 additions & 0 deletions cvp_mesh_planner/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,13 @@
cmake_minimum_required(VERSION 3.8)
project(cvp_mesh_planner)

# DEFAULT RELEASE
if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
if (NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
endif()
endif()

find_package(ament_cmake_ros REQUIRED)
find_package(mbf_mesh_core REQUIRED)
find_package(mbf_msgs REQUIRED)
Expand Down
95 changes: 50 additions & 45 deletions cvp_mesh_planner/src/cvp_mesh_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,18 +65,20 @@ uint32_t CVPMeshPlanner::makePlan(const geometry_msgs::msg::PoseStamped& start,
std::vector<geometry_msgs::msg::PoseStamped>& plan, double& cost,
std::string& message)
{
const auto& mesh = mesh_map_->mesh();
const auto mesh = mesh_map_->mesh();
std::list<std::pair<mesh_map::Vector, lvr2::FaceHandle>> path;

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

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

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

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

RCLCPP_DEBUG_STREAM(node_->get_logger(), "finished wave front propagation.");

path.reverse();

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

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

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

void CVPMeshPlanner::computeVectorMap()
{
const auto& mesh = mesh_map_->mesh();
const auto mesh = mesh_map_->mesh();
const auto& face_normals = mesh_map_->faceNormals();
const auto& vertex_normals = mesh_map_->vertexNormals();

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

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

const auto& vec3 = mesh.getVertexPosition(v3);
const auto& vec1 = mesh.getVertexPosition(v1);
const auto& vec3 = mesh->getVertexPosition(v3);
const auto& vec1 = mesh->getVertexPosition(v1);

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

const double u1 = distances[v1];
const double u2 = distances[v2];
const double u3 = distances[v3];

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

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

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

Expand Down Expand Up @@ -284,7 +286,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
predecessors_[v3] = v1;
direction_[v3] = static_cast<float>(theta);
distances[v3] = static_cast<float>(u3tmp);
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fh);
#ifdef DEBUG
mesh_map->publishDebugVector(v3, v1, fh, theta, mesh_map::color(0.9, 0.9, 0.2),
Expand All @@ -300,7 +302,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
predecessors_[v3] = v1;
direction_[v3] = 0;
distances[v3] = u3tmp;
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fh);
#ifdef DEBUG
mesh_map->publishDebugVector(v3, v1, fh, 0, mesh_map::color(0.9, 0.9, 0.2),
Expand All @@ -317,7 +319,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
const double t2cos = (a_sq + u3tmp_sq - u2_sq) / (2 * a * u3tmp);
if (S <= 0 && std::fabs(t2cos) <= 1)
{
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
const double theta = -acos(t2cos);
direction_[v3] = static_cast<float>(theta);
distances[v3] = static_cast<float>(u3tmp);
Expand All @@ -337,7 +339,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateWithS(lvr2::DenseVertexMap<float>& di
direction_[v3] = 0;
distances[v3] = u3tmp;
predecessors_[v3] = v2;
const lvr2::FaceHandle fh = mesh.getFaceBetween(v1, v2, v3).unwrap();
const lvr2::FaceHandle fh = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fh);
#ifdef DEBUG
mesh_map->publishDebugVector(v3, v2, fh, 0, mesh_map::color(0.9, 0.9, 0.2),
Expand All @@ -357,21 +359,21 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
const lvr2::VertexHandle& v1, const lvr2::VertexHandle& v2,
const lvr2::VertexHandle& v3)
{
const auto& mesh = mesh_map_->mesh();
const auto mesh = mesh_map_->mesh();

const double u1 = distances[v1];
const double u2 = distances[v2];
const double u3 = distances[v3];

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

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

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

Expand Down Expand Up @@ -406,7 +408,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
u3tmp = u1 + b;
if (u3tmp < u3)
{
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v1;
#ifdef DEBUG
Expand All @@ -425,7 +427,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
u3tmp = u2 + a;
if (u3tmp < u3)
{
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v2;
#ifdef DEBUG
Expand Down Expand Up @@ -478,7 +480,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc

if (theta1 < theta0 && theta2 < theta0)
{
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
distances[v3] = static_cast<float>(u3tmp);
if (theta1 < theta2)
Expand Down Expand Up @@ -506,7 +508,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
u3tmp = u1 + b;
if (u3tmp < u3)
{
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v1;
distances[v3] = static_cast<float>(u3tmp);
Expand All @@ -524,7 +526,7 @@ inline bool CVPMeshPlanner::waveFrontUpdate(lvr2::DenseVertexMap<float>& distanc
u3tmp = u2 + a;
if (u3tmp < u3)
{
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v2;
distances[v3] = static_cast<float>(u3tmp);
Expand All @@ -549,7 +551,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
const lvr2::VertexHandle &v2tmp,
const lvr2::VertexHandle &v3)
{
const auto& mesh = mesh_map_->mesh();
const auto mesh = mesh_map_->mesh();

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

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

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

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

Expand Down Expand Up @@ -596,7 +598,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
const double u3_tmp = u1 + t;
if(u3_tmp < u3)
{
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v1;
distances[v3] = static_cast<float>(u3_tmp);
Expand All @@ -612,7 +614,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(

if (u1t < u2t) {
if (u1t < u3) {
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v1;
distances[v3] = static_cast<float>(u1t);
Expand All @@ -621,7 +623,7 @@ inline bool CVPMeshPlanner::waveFrontUpdateFMM(
}
} else {
if (u2t < u3) {
auto fH = mesh.getFaceBetween(v1, v2, v3).unwrap();
auto fH = mesh->getFaceBetween(v1, v2, v3).unwrap();
cutting_faces_.insert(v3, fH);
predecessors_[v3] = v2;
distances[v3] = static_cast<float>(u2t);
Expand All @@ -645,7 +647,7 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
{
RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init wave front propagation.");

const auto& mesh = mesh_map_->mesh();
const auto mesh = mesh_map_->mesh();
const auto& vertex_costs = mesh_map_->vertexCosts();
auto& invalid = mesh_map_->invalid;

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

// Find the containing faces of start and goal
const auto& start_opt = mesh_map_->getContainingFace(start, 0.4);
const auto& goal_opt = mesh_map_->getContainingFace(goal, 0.4);
const lvr2::OptionalFaceHandle start_opt = mesh_map_->getContainingFace(start, 0.4);
const lvr2::OptionalFaceHandle goal_opt = mesh_map_->getContainingFace(goal, 0.4);

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

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

if (!start_opt) {
message = "Could not find a face close enough to the given start pose";
RCLCPP_WARN_STREAM(node_->get_logger(), "waveFrontPropagation(): " << message);
return mbf_msgs::action::GetPath::Result::INVALID_START;
}
if (!goal_opt) {
message = "Could not find a face close enough to the given goal pose";
RCLCPP_WARN_STREAM(node_->get_logger(), "waveFrontPropagation(): " << message);
return mbf_msgs::action::GetPath::Result::INVALID_GOAL;
}

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

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

// clear vector field map
vector_map_.clear();

RCLCPP_DEBUG_STREAM(node_->get_logger(), "Init distances.");
// initialize distances with infinity
// initialize predecessor of each vertex with itself
for (auto const& vH : mesh.vertices())
for (auto const& vH : mesh->vertices())
{
distances.insert(vH, std::numeric_limits<float>::infinity());
predecessors.insert(vH, vH);
Expand All @@ -699,9 +704,9 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
lvr2::Meap<lvr2::VertexHandle, float> pq;
// Set start distance to zero
// add start vertex to priority queue
for (auto vH : mesh.getVerticesOfFace(start_face))
for (auto vH : mesh->getVerticesOfFace(start_face))
{
const mesh_map::Vector diff = start - mesh.getVertexPosition(vH);
const mesh_map::Vector diff = start - mesh->getVertexPosition(vH);
const float dist = diff.length();
distances[vH] = dist;
vector_map_.insert(vH, diff);
Expand All @@ -710,13 +715,13 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
pq.insert(vH, dist);
}

std::array<lvr2::VertexHandle, 3> goal_vertices = mesh.getVerticesOfFace(goal_face);
std::array<lvr2::VertexHandle, 3> goal_vertices = mesh->getVerticesOfFace(goal_face);
RCLCPP_DEBUG_STREAM(node_->get_logger(), "The goal is at (" << goal.x << ", " << goal.y << ", " << goal.z << ") at the face ("
<< goal_vertices[0] << ", " << goal_vertices[1] << ", " << goal_vertices[2]
<< ")");
mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1");
mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2");
mesh_map_->publishDebugPoint(mesh.getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3");
mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[0]), mesh_map::color(0, 0, 1), "goal_face_v1");
mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[1]), mesh_map::color(0, 0, 1), "goal_face_v2");
mesh_map_->publishDebugPoint(mesh->getVertexPosition(goal_vertices[2]), mesh_map::color(0, 0, 1), "goal_face_v3");

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

Expand Down Expand Up @@ -756,11 +761,11 @@ uint32_t CVPMeshPlanner::waveFrontPropagation(const mesh_map::Vector& original_s
try
{
std::vector<lvr2::FaceHandle> faces;
mesh.getFacesOfVertex(current_vh, faces);
mesh->getFacesOfVertex(current_vh, faces);

for (auto fh : faces)
{
const auto vertices = mesh.getVerticesOfFace(fh);
const auto vertices = mesh->getVerticesOfFace(fh);
const lvr2::VertexHandle& a = vertices[0];
const lvr2::VertexHandle& b = vertices[1];
const lvr2::VertexHandle& c = vertices[2];
Expand Down
7 changes: 7 additions & 0 deletions dijkstra_mesh_planner/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,13 @@
cmake_minimum_required(VERSION 3.8)
project(dijkstra_mesh_planner)

# DEFAULT RELEASE
if (NOT EXISTS ${CMAKE_BINARY_DIR}/CMakeCache.txt)
if (NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE)
endif()
endif()

find_package(ament_cmake_ros REQUIRED)
find_package(mbf_mesh_core REQUIRED)
find_package(mbf_msgs REQUIRED)
Expand Down
Loading

0 comments on commit b36f715

Please sign in to comment.