Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

LVR Update #70

Merged
merged 10 commits into from
Jan 2, 2025
Merged

LVR Update #70

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
5f5a1e9
started to adapt to lvr update
amock Dec 5, 2024
bf6bba1
compiles with new version of lvr now
amock Dec 6, 2024
37abc56
almost the same state as before
amock Dec 6, 2024
1078da7
You can select the half-edge-mesh implementation via the parameter 'h…
amock Dec 9, 2024
a46c2d9
Merge branch 'humble' into lvr-update
amock Dec 11, 2024
8c0e77f
reenabled info stats for mesh loading
amock Dec 12, 2024
d0d39cd
added lvr update branch to CI
amock Dec 20, 2024
b3c77c6
added lvr update branch to CI
amock Dec 20, 2024
22a568d
changed mesh tools branch in source dependencies
amock Dec 20, 2024
3f2e411
removed debug logs
amock Dec 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions cvp_mesh_planner/CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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.");
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice, I appreciate using proper log levels! 👍


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
Open in desktop
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
Loading