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

Node logging in moveit_core #2503

Merged
merged 9 commits into from
Dec 5, 2023
Merged

Node logging in moveit_core #2503

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
c42f73f
Node logging in moveit_core
tylerjw Oct 31, 2023
2503535
prototype in logger_from_child_dut
tylerjw Nov 1, 2023
d83b147
yet another refactor
tylerjw Nov 2, 2023
368c617
update migration guide
tylerjw Nov 2, 2023
e1c0709
rebase
tylerjw Nov 27, 2023
c5c7e81
Fix clang-tidy wanrings
tylerjw Nov 27, 2023
3e3a622
apply changes from review
tylerjw Dec 4, 2023
ece2ab2
Merge branch 'main' into logger_moveit_core
tylerjw Dec 5, 2023
2cec4ef
Update moveit_ros/move_group/src/default_capabilities/state_validatio…
tylerjw Dec 5, 2023
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
20 changes: 3 additions & 17 deletions doc/MIGRATION_GUIDE.md
View file
Open in desktop
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");
Copy link
Contributor

Choose a reason for hiding this comment

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

Honestly very stupid question:

Suggested change
RCLCPP_INFO(moveit::getLogger("my_namespace"), "Very important info message");
moveit::logInfo("my_namespace", "Very important info message");

or

Suggested change
RCLCPP_INFO(moveit::getLogger("my_namespace"), "Very important info message");
MOVEIT_INFO("my_namespace", "Very important info message");

do read loads better, right?

I would clearly support a readable wrapper over an awkward combination of MoveIt internal static magic (definitely not what god intended) TOGETHER with C macro magic (which was clearly invented by demons).

Copy link
Member Author

Choose a reason for hiding this comment

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

Here is the issues I see with those two approaches.

functions

The macros capture the file, function, and line number from the place where the macro is called. If we wrapped the ros logging system in our own functions we would loose that.

our own macros

This is a more ergonomic solution but would require me to write macros that wrap the ros macros. That would be a lot of code to write, test, and maintain. I think this approach is better given that tradeoff.

Copy link
Contributor

Choose a reason for hiding this comment

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

Fair enough. We could probably limit a convenience macro wrapper to the plain variants for almost all uses. That approach works well in OMPL and surely others. But then the question would be whether stream or printf-style and combinations might quickly blow up again if people disagree.

Still, an argument slot that only ever contains one generic term in correct usage should not exist either.

Between a rock and a hard place by design.

Copy link
Member Author

Choose a reason for hiding this comment

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

Yea, I think in this case the ROS 2 logging interface is just worse from a usability standpoint. I want to reduce the amount of abstractions I maintain so I don't want to maintain a set of wrapper logging macros. For that reason I think what is in this PR is the best compromise for now.

```

### Logger naming convention
Expand Down
1 change: 1 addition & 0 deletions moveit_core/collision_detection/CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
// 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,
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
// %.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
Loading