Skip to content

Commit

Permalink
Node logging in moveit_core (#2503)
Browse files Browse the repository at this point in the history
  • Loading branch information
@tylerjw
tylerjw authored Dec 5, 2023
1 parent 7ab329d commit 74debfd
Show file tree
Hide file tree
Showing 155 changed files with 1,327 additions and 1,133 deletions.
20 changes: 3 additions & 17 deletions doc/MIGRATION_GUIDE.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,29 +13,15 @@ However because of initaliziation order (child logger has to be created after rc
#include <moveit/utils/logger.hpp>
```

Wherever the node is created there is the option to set the global logger for moveit using this syntax:
Wherever the node is created there is the option to set the root logging namespace to be from the node for moveit using this syntax:

```C++
moveit::setLogger(node->get_logger());
moveit::setNodeLoggerName(node->get_name());
```
Then wherever you call a logging macro you can use the `moveit::getLogger()` function:
```C++
RCLCPP_INFO(moveit::getLogger(), "Very important info message");
```

To have namespaces you need to create a child logger.
There is a function for that too.
This creates a child of the global logger.
You'll find this in the constructor of many of our classes.

```C++
, logger_(moveit::makeChildLogger("servo"))
```

Once you have a child logger you can use it in logging macros:
```C++
RCLCPP_INFO(logger_, "Very important info message");
RCLCPP_INFO(moveit::getLogger("my_namespace"), "Very important info message");
```

### Logger naming convention
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
88 changes: 48 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,111 @@
#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()
{
return moveit::getLogger("collision_detection.world_allvalid");
}
} // 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
16 changes: 12 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,37 @@
#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()
{
return moveit::getLogger("collision_detection_common");
}
} // 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
18 changes: 12 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,26 @@
#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()
{
return moveit::getLogger("collision_detection_env");
}
} // 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 +67,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
15 changes: 11 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,17 @@
#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()
{
return moveit::getLogger("collision_detection_matrix");
}
} // namespace

AllowedCollisionMatrix::AllowedCollisionMatrix()
{
Expand Down Expand Up @@ -76,7 +82,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 +95,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
26 changes: 16 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,15 @@
#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()
{
return moveit::getLogger("collision_detection_octomap_filter");
}
} // namespace

// forward declarations
bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value,
Expand All @@ -64,12 +70,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 +135,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 +157,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 +274,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 +300,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 74debfd

Please sign in to comment.