Skip to content

Commit

Permalink
collision_distance_filed, adapt logging to moveit#21
Browse files Browse the repository at this point in the history
  • Loading branch information
@vmayoral
vmayoral committed Mar 31, 2019
1 parent a61f85e commit 6eb216e
Show file tree
Hide file tree
Showing 8 changed files with 65 additions and 70 deletions.
5 changes: 0 additions & 5 deletions moveit_core/collision_distance_field/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,5 @@
set(MOVEIT_LIB_NAME moveit_collision_distance_field)

if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
# clang is picky about new virtual functions hiding existing ones
add_compile_options(-Wno-overloaded-virtual)
endif()

add_library(${MOVEIT_LIB_NAME}
src/collision_distance_field_types.cpp
src/collision_common_distance_field.cpp
Expand Down
10 changes: 5 additions & 5 deletions ...n_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@
#include "rclcpp/time.hpp"
#include "rclcpp/utilities.hpp"

rclcpp::Logger logger = rclcpp::get_logger("collision_distance_field");
rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("collision_distance_field");

namespace collision_detection
{
Expand Down Expand Up @@ -411,7 +411,7 @@ class PosedBodySphereDecompositionVector
{
if (i >= decomp_vector_.size())
{
RCLCPP_INFO(logger, "No body decomposition");
RCLCPP_INFO(LOGGER, "No body decomposition");
return empty_ptr_;
}
return decomp_vector_[i];
Expand All @@ -421,7 +421,7 @@ class PosedBodySphereDecompositionVector
{
if (ind >= decomp_vector_.size())
{
RCLCPP_WARN(logger, "Can't update pose");
RCLCPP_WARN(LOGGER, "Can't update pose");
return;
}
decomp_vector_[ind]->updatePose(pose);
Expand Down Expand Up @@ -474,7 +474,7 @@ class PosedBodyPointDecompositionVector
{
if (i >= decomp_vector_.size())
{
RCLCPP_INFO(logger, "No body decomposition");
RCLCPP_INFO(LOGGER, "No body decomposition");
return empty_ptr_;
}
return decomp_vector_[i];
Expand All @@ -488,7 +488,7 @@ class PosedBodyPointDecompositionVector
}
else
{
RCLCPP_WARN(logger, "Can't update pose");
RCLCPP_WARN(LOGGER, "Can't update pose");
return;
}
}
Expand Down
16 changes: 8 additions & 8 deletions ...n_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,37 +103,37 @@ class CollisionRobotDistanceField : public CollisionRobot
void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
};

void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& state1, const moveit::core::RobotState& state2,
const collision_detection::AllowedCollisionMatrix& acm) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
};

void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& state, const CollisionRobot& other_robot,
const moveit::core::RobotState& other_state) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
};

void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& state, const CollisionRobot& other_robot,
const moveit::core::RobotState& other_state,
const collision_detection::AllowedCollisionMatrix& acm) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
};

void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
const moveit::core::RobotState& state1, const moveit::core::RobotState& state2,
const CollisionRobot& other_robot, const moveit::core::RobotState& other_state1,
const moveit::core::RobotState& other_state2) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
};

void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
Expand All @@ -142,7 +142,7 @@ class CollisionRobotDistanceField : public CollisionRobot
const moveit::core::RobotState& other_state2,
const collision_detection::AllowedCollisionMatrix& acm) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
};

void createCollisionModelMarker(const moveit::core::RobotState& state,
Expand Down Expand Up @@ -172,13 +172,13 @@ class CollisionRobotDistanceField : public CollisionRobot
void distanceSelf(const DistanceRequest& req, DistanceResult& res,
const robot_state::RobotState& state) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
}

void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
}

DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const
Expand Down
4 changes: 2 additions & 2 deletions ...n_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -145,12 +145,12 @@ class CollisionWorldDistanceField : public CollisionWorld
void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
const robot_state::RobotState& state) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
}

void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override
{
RCLCPP_ERROR(logger, "Not implemented");
RCLCPP_ERROR(LOGGER, "Not implemented");
}

void setWorld(const WorldPtr& world) override;
Expand Down
4 changes: 2 additions & 2 deletions moveit_core/collision_distance_field/src/collision_common_distance_field.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,15 +193,15 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g
const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
if (!att)
{
RCLCPP_WARN(logger,"Attached body '%s' was not found, skipping sphere "
RCLCPP_WARN(LOGGER,"Attached body '%s' was not found, skipping sphere "
"decomposition visualization",
gsr->dfce_->attached_body_names_[i].c_str());
continue;
}

if (gsr->attached_body_decompositions_[i]->getSize() != att->getShapes().size())
{
RCLCPP_WARN(logger,"Attached body size discrepancy");
RCLCPP_WARN(LOGGER,"Attached body size discrepancy");
continue;
}

Expand Down
16 changes: 8 additions & 8 deletions moveit_core/collision_distance_field/src/collision_distance_field_types.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ bool collision_detection::getCollisionSphereGradients(const distance_field::Dist
double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
if (!in_bounds && grad.norm() > EPSILON)
{
RCLCPP_DEBUG(logger, "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
return true;
}

Expand Down Expand Up @@ -202,7 +202,7 @@ bool collision_detection::getCollisionSphereCollision(const distance_field::Dist

if (!in_bounds && grad.norm() > 0)
{
RCLCPP_DEBUG(logger,"Collision sphere point is out of bounds");
RCLCPP_DEBUG(LOGGER,"Collision sphere point is out of bounds");
return true;
}

Expand Down Expand Up @@ -230,7 +230,7 @@ bool collision_detection::getCollisionSphereCollision(const distance_field::Dist
double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
if (!in_bounds && (grad.norm() > 0))
{
RCLCPP_DEBUG(logger, "Collision sphere point is out of bounds");
RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds");
return true;
}
if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
Expand Down Expand Up @@ -314,7 +314,7 @@ void collision_detection::BodyDecomposition::init(const std::vector<shapes::Shap
}
bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_);

RCLCPP_DEBUG(logger, "BodyDecomposition generated %i collision spheres out of %i shapes",
RCLCPP_DEBUG(LOGGER, "BodyDecomposition generated %i collision spheres out of %i shapes",
collision_spheres_.size(), shapes.size());
}

Expand Down Expand Up @@ -444,7 +444,7 @@ void collision_detection::getProximityGradientMarkers(
rclcpp::Clock ros_clock;
if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
{
RCLCPP_WARN(logger, "Size mismatch between gradients %u and decompositions %u",
RCLCPP_WARN(LOGGER, "Size mismatch between gradients %u and decompositions %u",
(unsigned int)gradients.size(),
(unsigned int)(posed_decompositions.size() + posed_vector_decompositions.size()));
return;
Expand Down Expand Up @@ -478,13 +478,13 @@ void collision_detection::getProximityGradientMarkers(
}
else
{
RCLCPP_DEBUG(logger, "Negative length for %u %d %lf", i, arrow_mark.id,
RCLCPP_DEBUG(LOGGER, "Negative length for %u %d %lf", i, arrow_mark.id,
gradients[i].gradients[j].norm());
}
}
else
{
RCLCPP_DEBUG(logger , "Negative dist %lf for %u %d", gradients[i].distances[j], i,
RCLCPP_DEBUG(LOGGER , "Negative dist %lf for %u %d", gradients[i].distances[j], i,
arrow_mark.id);
}
arrow_mark.points.resize(2);
Expand Down Expand Up @@ -548,7 +548,7 @@ void collision_detection::getCollisionMarkers(
rclcpp::Clock ros_clock;
if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
{
RCLCPP_WARN(logger, "Size mismatch between gradients %zu and decompositions %zu",
RCLCPP_WARN(LOGGER, "Size mismatch between gradients %zu and decompositions %zu",
gradients.size(), posed_decompositions.size() + posed_vector_decompositions.size());
return;
}
Expand Down
Loading

0 comments on commit 6eb216e

Please sign in to comment.