Skip to content

Commit

Permalink
Node logging in moveit_core
Browse files Browse the repository at this point in the history
  • Loading branch information
@tylerjw
tylerjw committed Oct 31, 2023
1 parent 87e3f91 commit 57ced77
Show file tree
Hide file tree
Showing 64 changed files with 970 additions and 653 deletions.
14 changes: 14 additions & 0 deletions doc/MIGRATION_GUIDE.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,20 @@ Once you have a child logger you can use it in logging macros:
RCLCPP_INFO(logger_, "Very important info message");
```
In some files you'll find the creation of a static logger for the file like this.
Note that this is different from the previous file level static variables because the logger is not initalized until the function is called the first time.
This enables us to set the global node logger before this is called.
```C++
namespace
{
rclcpp::Logger getLogger()
{
static auto logger = moveit::makeChildLogger("moveit_collision_detection");
return logger;
}
} // namespace
```

### Logger naming convention

Migrating the loggers is a good opportunity to make logger names more consistent.
Expand Down
1 change: 1 addition & 0 deletions moveit_core/collision_detection/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ set_target_properties(moveit_collision_detection PROPERTIES VERSION ${${PROJECT_

target_link_libraries(moveit_collision_detection
moveit_robot_state
moveit_utils
)

# unit tests
Expand Down
89 changes: 49 additions & 40 deletions moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,103 +38,112 @@
#include <moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <moveit/utils/logger.hpp>

// Logger
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_world_allvalid");
namespace collision_detection
{
namespace
{
rclcpp::Logger getLogger()
{
static auto logger = moveit::makeChildLogger("collision_detection_world_allvalid");
return logger;
}
} // namespace

const std::string collision_detection::CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");
const std::string CollisionDetectorAllocatorAllValid::NAME("ALL_VALID");

collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model,
double padding, double scale)
CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, double padding,
double scale)
: CollisionEnv(robot_model, padding, scale)
{
}

collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model,
const WorldPtr& world, double padding, double scale)
CollisionEnvAllValid::CollisionEnvAllValid(const moveit::core::RobotModelConstPtr& robot_model, const WorldPtr& world,
double padding, double scale)
: CollisionEnv(robot_model, world, padding, scale)
{
}

collision_detection::CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
CollisionEnvAllValid::CollisionEnvAllValid(const CollisionEnv& other, const WorldPtr& world)
: CollisionEnv(other, world)
{
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/) const
{
res.collision = false;
if (req.verbose)
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/,
const AllowedCollisionMatrix& /*acm*/) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/,
const AllowedCollisionMatrix& /*acm*/) const
{
res.collision = false;
if (req.verbose)
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state1*/,
const moveit::core::RobotState& /*state2*/) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state1*/,
const moveit::core::RobotState& /*state2*/) const
{
res.collision = false;
if (req.verbose)
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state1*/,
const moveit::core::RobotState& /*state2*/,
const AllowedCollisionMatrix& /*acm*/) const
void CollisionEnvAllValid::checkRobotCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state1*/,
const moveit::core::RobotState& /*state2*/,
const AllowedCollisionMatrix& /*acm*/) const
{
res.collision = false;
if (req.verbose)
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::distanceRobot(const collision_detection::DistanceRequest& /*req*/,
collision_detection::DistanceResult& res,
const moveit::core::RobotState& /*state*/) const
void CollisionEnvAllValid::distanceRobot(const DistanceRequest& /*req*/, DistanceResult& res,
const moveit::core::RobotState& /*state*/) const
{
res.collision = false;
}

double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const
double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/) const
{
return 0.0;
}

double collision_detection::CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/,
const AllowedCollisionMatrix& /*acm*/) const
double CollisionEnvAllValid::distanceRobot(const moveit::core::RobotState& /*state*/,
const AllowedCollisionMatrix& /*acm*/) const
{
return 0.0;
}

void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/) const
void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/) const
{
res.collision = false;
if (req.verbose)
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/,
const AllowedCollisionMatrix& /*acm*/) const
void CollisionEnvAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const moveit::core::RobotState& /*state*/,
const AllowedCollisionMatrix& /*acm*/) const
{
res.collision = false;
if (req.verbose)
RCLCPP_INFO(LOGGER, "Using AllValid collision detection. No collision checking is performed.");
RCLCPP_INFO(getLogger(), "Using AllValid collision detection. No collision checking is performed.");
}

void collision_detection::CollisionEnvAllValid::distanceSelf(const collision_detection::DistanceRequest& /*req*/,
collision_detection::DistanceResult& res,
const moveit::core::RobotState& /*state*/) const
void CollisionEnvAllValid::distanceSelf(const DistanceRequest& /*req*/, DistanceResult& res,
const moveit::core::RobotState& /*state*/) const
{
res.collision = false;
}

} // namespace collision_detection
17 changes: 13 additions & 4 deletions moveit_core/collision_detection/src/collision_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,29 +36,38 @@
#include <rclcpp/clock.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <moveit/utils/logger.hpp>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_common");
constexpr size_t LOG_THROTTLE_PERIOD{ 5 };

namespace collision_detection
{
namespace
{
rclcpp::Logger getLogger()
{
static auto logger = moveit::makeChildLogger("collision_detection_common");
return logger;
}
} // namespace

void CollisionResult::print() const
{
rclcpp::Clock clock;
if (!contacts.empty())
{
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wold-style-cast"
RCLCPP_WARN_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD,
RCLCPP_WARN_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD,
"Objects in collision (printing 1st of "
<< contacts.size() << " pairs): " << contacts.begin()->first.first << ", "
<< contacts.begin()->first.second);

// Log all collisions at the debug level
RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD, "Objects in collision:");
RCLCPP_DEBUG_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD, "Objects in collision:");
for (const auto& contact : contacts)
{
RCLCPP_DEBUG_STREAM_THROTTLE(LOGGER, clock, LOG_THROTTLE_PERIOD,
RCLCPP_DEBUG_STREAM_THROTTLE(getLogger(), clock, LOG_THROTTLE_PERIOD,
"\t" << contact.first.first << ", " << contact.first.second);
}
#pragma GCC diagnostic pop
Expand Down
19 changes: 13 additions & 6 deletions moveit_core/collision_detection/src/collision_env.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,20 +38,27 @@
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <limits>
#include <moveit/utils/logger.hpp>

// Logger
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_robot");
namespace
{
rclcpp::Logger getLogger()
{
static auto logger = moveit::makeChildLogger("collision_detection_env");
return logger;
}
} // namespace

static inline bool validateScale(const double scale)
{
if (scale < std::numeric_limits<double>::epsilon())
{
RCLCPP_ERROR(LOGGER, "Scale must be positive");
RCLCPP_ERROR(getLogger(), "Scale must be positive");
return false;
}
if (scale > std::numeric_limits<double>::max())
{
RCLCPP_ERROR(LOGGER, "Scale must be finite");
RCLCPP_ERROR(getLogger(), "Scale must be finite");
return false;
}
return true;
Expand All @@ -61,12 +68,12 @@ static inline bool validatePadding(const double padding)
{
if (padding < 0.0)
{
RCLCPP_ERROR(LOGGER, "Padding cannot be negative");
RCLCPP_ERROR(getLogger(), "Padding cannot be negative");
return false;
}
if (padding > std::numeric_limits<double>::max())
{
RCLCPP_ERROR(LOGGER, "Padding must be finite");
RCLCPP_ERROR(getLogger(), "Padding must be finite");
return false;
}
return true;
Expand Down
16 changes: 12 additions & 4 deletions moveit_core/collision_detection/src/collision_matrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,18 @@
#include <rclcpp/logging.hpp>
#include <functional>
#include <iomanip>
#include <moveit/utils/logger.hpp>

namespace collision_detection
{
// Logger
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_matrix");
namespace
{
rclcpp::Logger getLogger()
{
static auto logger = moveit::makeChildLogger("collision_detection_matrix");
return logger;
}
} // namespace

AllowedCollisionMatrix::AllowedCollisionMatrix()
{
Expand Down Expand Up @@ -76,7 +83,8 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo
if (msg.entry_names.size() != msg.entry_values.size() ||
msg.default_entry_names.size() != msg.default_entry_values.size())
{
RCLCPP_ERROR(LOGGER, "The number of links does not match the number of entries in AllowedCollisionMatrix message");
RCLCPP_ERROR(getLogger(),
"The number of links does not match the number of entries in AllowedCollisionMatrix message");
return;
}
for (std::size_t i = 0; i < msg.default_entry_names.size(); ++i)
Expand All @@ -88,7 +96,7 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCo
{
if (msg.entry_values[i].enabled.size() != msg.entry_names.size())
{
RCLCPP_ERROR(LOGGER, "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
RCLCPP_ERROR(getLogger(), "Number of entries is incorrect for link '%s' in AllowedCollisionMatrix message",
msg.entry_names[i].c_str());
return;
}
Expand Down
27 changes: 17 additions & 10 deletions moveit_core/collision_detection/src/collision_octomap_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,16 @@
#include <rclcpp/logger.hpp>
#include <rclcpp/logging.hpp>
#include <memory>
#include <moveit/utils/logger.hpp>

// Logger
static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_octomap_filter");
namespace
{
rclcpp::Logger getLogger()
{
static auto logger = moveit::makeChildLogger("collision_detection_octomap_filter");
return logger;
}
} // namespace

// forward declarations
bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value,
Expand All @@ -64,12 +71,12 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
{
if (!object)
{
RCLCPP_ERROR(LOGGER, "No valid Object passed in, cannot refine Normals!");
RCLCPP_ERROR(getLogger(), "No valid Object passed in, cannot refine Normals!");
return 0;
}
if (res.contact_count < 1)
{
RCLCPP_WARN(LOGGER, "There do not appear to be any contacts, so there is nothing to refine!");
RCLCPP_WARN(getLogger(), "There do not appear to be any contacts, so there is nothing to refine!");
return 0;
}

Expand Down Expand Up @@ -129,16 +136,16 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
{
count++;
node_centers.push_back(pt);
// RCLCPP_INFO(LOGGER, "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
// RCLCPP_INFO(getLogger(), "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]",
// count, prob, pt.x(), pt.y(), pt.z());
}
}
// RCLCPP_INFO(LOGGER, "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
// RCLCPP_INFO(getLogger(), "Contact point at [%.3f, %.3f, %.3f], cell size %.3f, occupied cells
// %d",
// contact_point.x(), contact_point.y(), contact_point.z(), cell_size, count);

// octree->getOccupiedLeafsBBX(node_centers, bbx_min, bbx_max);
// RCLCPP_ERROR(LOGGER, "bad stuff in collision_octomap_filter.cpp; need to port octomap
// RCLCPP_ERROR(getLogger(), "bad stuff in collision_octomap_filter.cpp; need to port octomap
// call for groovy");

octomath::Vector3 n;
Expand All @@ -151,7 +158,7 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec
if (divergence > allowed_angle_divergence)
{
modified++;
// RCLCPP_INFO(LOGGER, "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
// RCLCPP_INFO(getLogger(), "Normals differ by %.3f, changing: [%.3f, %.3f, %.3f] -> [%.3f,
// %.3f, %.3f]",
// divergence, contact_normal.x(), contact_normal.y(), contact_normal.z(),
// n.x(), n.y(), n.z());
Expand Down Expand Up @@ -268,7 +275,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_mu
}
else
{
RCLCPP_ERROR(LOGGER, "This should not be called!");
RCLCPP_ERROR(getLogger(), "This should not be called!");
}

double f_val = 0;
Expand All @@ -294,7 +301,7 @@ bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_mu
}
else
{
RCLCPP_ERROR(LOGGER, "This should not be called!");
RCLCPP_ERROR(getLogger(), "This should not be called!");
const double r_scaled = r / r;
// TODO still need to address the scaling...
f_val = pow((1 - r_scaled), 4) * (4 * r_scaled + 1);
Expand Down
Loading

0 comments on commit 57ced77

Please sign in to comment.