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

Jump to bottom

Port collision_detection_fcl to ROS 2 #41

Merged
merged 20 commits into from
Jun 12, 2019
Merged

Port collision_detection_fcl to ROS 2 #41

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
20 commits
Select commit Hold shift + click to select a range
90fc0cb
Port collision_detection_fcl to ROS 2
vmayoral Mar 10, 2019
d4e505e
collision_detection_fcl, fix CMakeLists.txt
vmayoral Mar 30, 2019
5a169f5
Merge branch 'moveit2' into collision-detection-fcl
vmayoral Mar 30, 2019
615642d
xcollision_detector_fcl, missing dependencies
vmayoral Mar 30, 2019
534f026
collision_detection_fcl, remove leftovers from logger prototype
vmayoral Mar 30, 2019
ffaf3f5
collision_detection_fcl - renamed NAME to NAME_
ahcorde May 16, 2019
183c9a9
Collision_detection_fcl - fixing logger variables name
ahcorde May 16, 2019
17b79ef
collision_detection_fcl - porting test_fcl_collision_detection.cpp
ahcorde May 16, 2019
195e319
collision_detection_fcl - adding tests and dependencies to CMakeLists…
ahcorde May 16, 2019
efbbfd6
Update moveit_core/collision_detection_fcl/include/moveit/collision_d…
May 16, 2019
fbd492f
Update moveit_core/collision_detection_fcl/CMakeLists.txt
May 16, 2019
d1c729b
clang
May 21, 2019
265675c
tabs to spaces
May 21, 2019
a3011f8
moveit_core collision detection_fcl - loggers inside namespace
ahcorde May 24, 2019
18a607d
moveit_core collision_detection_fcl - revert NAME_ to NAME
ahcorde May 24, 2019
ed4f512
Fix collision_detection logger
anasarrak Jun 3, 2019
cbc0b48
Fix fix_fcl_collision_detection
anasarrak Jun 3, 2019
86c1614
move logger collision_common
anasarrak Jun 3, 2019
17688f0
Revert back logger
anasarrak Jun 3, 2019
992fef0
clang format
anasarrak Jun 3, 2019
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
65 changes: 50 additions & 15 deletions moveit_core/collision_detection_fcl/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,32 +1,67 @@
set(MOVEIT_LIB_NAME moveit_collision_detection_fcl)

add_library(${MOVEIT_LIB_NAME}
add_library(${MOVEIT_LIB_NAME} SHARED
src/collision_common.cpp
src/collision_robot_fcl.cpp
src/collision_world_fcl.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_collision_detection
rclcpp
rmw_implementation
urdf
urdfdom
urdfdom_headers
LIBFCL
Boost
visualization_msgs)

target_link_libraries(${MOVEIT_LIB_NAME} moveit_collision_detection ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${LIBFCL_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${MOVEIT_LIB_NAME}
moveit_collision_detection
)

add_library(collision_detector_fcl_plugin src/collision_detector_fcl_plugin_loader.cpp)
add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp)
set_target_properties(collision_detector_fcl_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(collision_detector_fcl_plugin ${catkin_LIBRARIES} ${MOVEIT_LIB_NAME})

ament_target_dependencies(collision_detector_fcl_plugin
rclcpp
urdf
visualization_msgs
pluginlib
rmw_implementation
${MOVEIT_LIB_NAME})

install(TARGETS ${MOVEIT_LIB_NAME} collision_detector_fcl_plugin
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})

install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
install(DIRECTORY include/ DESTINATION include)

install(FILES ../collision_detector_fcl_description.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

if(CATKIN_ENABLE_TESTING)
if(BUILD_TESTING)
find_package(moveit_resources REQUIRED)
include_directories(${moveit_resources_INCLUDE_DIRS})
find_package(resource_retriever REQUIRED)
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

if(WIN32)
# set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")
else()
set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils")
endif()

ament_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")

catkin_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection.cpp)
target_link_libraries(test_fcl_collision_detection moveit_test_utils ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})
target_link_libraries(test_fcl_collision_detection
${urdf_LIBRARIES}
${visualization_msgs_LIBRARIES}
${srdfdom_LIBRARIES}
moveit_test_utils
moveit_robot_model
moveit_robot_state
moveit_collision_detection
moveit_collision_detection_fcl
${MOVEIT_LIB_NAME}
${Boost_LIBRARIES}
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
)
endif()
1 change: 1 addition & 0 deletions ...it_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@
#include <memory>
#include <set>


vmayoral marked this conversation as resolved.
Show resolved Hide resolved
namespace collision_detection
{
MOVEIT_STRUCT_FORWARD(CollisionGeometryData);
Expand Down
2 changes: 1 addition & 1 deletion ...n_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ class CollisionDetectorAllocatorFCL
: public CollisionDetectorAllocatorTemplate<CollisionWorldFCL, CollisionRobotFCL, CollisionDetectorAllocatorFCL>
{
public:
static const std::string NAME; // defined in collision_world_fcl.cpp
static const std::string NAME_; // defined in collision_world_fcl.cpp
Copy link
Contributor

Choose a reason for hiding this comment

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

revert

Copy link
Contributor

Choose a reason for hiding this comment

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

18a607d

};
}

Expand Down
55 changes: 24 additions & 31 deletions moveit_core/collision_detection_fcl/src/collision_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@
#include <boost/thread/mutex.hpp>
#include <memory>

rclcpp::Logger LOGGER_COLLISION_DETECTION = rclcpp::get_logger("collision_detection.fcl");
Copy link
Contributor

Choose a reason for hiding this comment

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

move inside the namespace

Copy link
Contributor

Choose a reason for hiding this comment

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

a3011f8


namespace collision_detection
{
bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void* data)
Expand Down Expand Up @@ -96,16 +98,15 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
{
always_allow_collision = true;
if (cdata->req_->verbose)
ROS_DEBUG_NAMED(
"collision_detection.fcl", "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "
"No contacts are computed.",
cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(), cd2->getTypeString().c_str());
}
else if (type == AllowedCollision::CONDITIONAL)
{
cdata->acm_->getAllowedCollision(cd1->getID(), cd2->getID(), dcf);
if (cdata->req_->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl", "Collision between '%s' and '%s' is conditionally allowed",
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision between '%s' and '%s' is conditionally allowed",
cd1->getID().c_str(), cd2->getID().c_str());
}
}
Expand All @@ -119,8 +120,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
{
always_allow_collision = true;
if (cdata->req_->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
cd1->getID().c_str(), cd2->getID().c_str());
}
}
Expand All @@ -131,8 +131,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
{
always_allow_collision = true;
if (cdata->req_->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Robot link '%s' is allowed to touch attached object '%s'. No contacts are computed.",
cd2->getID().c_str(), cd1->getID().c_str());
}
}
Expand All @@ -148,7 +147,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
return false;

if (cdata->req_->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Actually checking collisions between %s and %s", cd1->getID().c_str(),
cd2->getID().c_str());

// see if we need to compute a contact
Expand Down Expand Up @@ -185,8 +184,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
if (num_contacts > 0)
{
if (cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl",
"Found %d contacts between '%s' and '%s'. "
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Found %d contacts between '%s' and '%s'. "
"These contacts will be evaluated to check if they are accepted or not",
num_contacts, cd1->getID().c_str(), cd2->getID().c_str());
Contact c;
Expand All @@ -206,12 +204,11 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
cdata->res_->contacts[pc].push_back(c);
cdata->res_->contact_count++;
if (cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl",
"Found unacceptable contact between '%s' and '%s'. Contact was stored.",
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Found unacceptable contact between '%s' and '%s'. Contact was stored.",
cd1->getID().c_str(), cd2->getID().c_str());
}
else if (cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl", "Found unacceptable contact between '%s' (type '%s') and '%s' "
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Found unacceptable contact between '%s' (type '%s') and '%s' "
"(type '%s'). Contact was stored.",
cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
cd2->getTypeString().c_str());
Expand Down Expand Up @@ -264,7 +261,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
}

if (cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl", "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), "
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Found %d contacts between '%s' (type '%s') and '%s' (type '%s'), "
"which constitute a collision. %d contacts will be stored",
num_contacts_initial, cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
cd2->getTypeString().c_str(), num_contacts);
Expand Down Expand Up @@ -309,7 +306,7 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
{
cdata->res_->collision = true;
if (cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl", "Found a contact between '%s' (type '%s') and '%s' (type '%s'), "
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Found a contact between '%s' (type '%s') and '%s' (type '%s'), "
"which constitutes a collision. "
"Contact information is not stored.",
cd1->getID().c_str(), cd1->getTypeString().c_str(), cd2->getID().c_str(),
Expand Down Expand Up @@ -339,16 +336,15 @@ bool collisionCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, voi
if (!cdata->req_->cost)
cdata->done_ = true;
if (cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl",
"Collision checking is considered complete (collision was found and %u contacts are stored)",
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Collision checking is considered complete (collision was found and %u contacts are stored)",
(unsigned int)cdata->res_->contact_count);
}

if (!cdata->done_ && cdata->req_->is_done)
{
cdata->done_ = cdata->req_->is_done(*cdata->res_);
if (cdata->done_ && cdata->req_->verbose)
ROS_INFO_NAMED("collision_detection.fcl", "Collision checking is considered complete due to external callback. "
RCLCPP_INFO(LOGGER_COLLISION_DETECTION, "Collision checking is considered complete due to external callback. "
"%s was found. %u contacts are stored.",
cdata->res_->collision ? "Collision" : "No collision", (unsigned int)cdata->res_->contact_count);
}
Expand Down Expand Up @@ -381,7 +377,7 @@ struct FCLShapeCache
map_.erase(it);
it = nit;
}
// ROS_DEBUG_NAMED("collision_detection.fcl", "Cleaning up cache for FCL objects that correspond to static
// RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Cleaning up cache for FCL objects that correspond to static
// shapes. Cache size
// reduced from %u
// to %u", from, (unsigned int)map_.size());
Expand Down Expand Up @@ -439,8 +435,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
{
always_allow_collision = true;
if (cdata->req->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Collision between '%s' and '%s' is always allowed. No distances are computed.",
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision between '%s' and '%s' is always allowed. No distances are computed.",
cd1->getID().c_str(), cd2->getID().c_str());
}
}
Expand All @@ -454,8 +449,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
{
always_allow_collision = true;
if (cdata->req->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
cd1->getID().c_str(), cd2->getID().c_str());
}
}
Expand All @@ -468,8 +462,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
{
always_allow_collision = true;
if (cdata->req->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl",
"Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Robot link '%s' is allowed to touch attached object '%s'. No distances are computed.",
cd2->getID().c_str(), cd1->getID().c_str());
}
}
Expand All @@ -480,7 +473,7 @@ bool distanceCallback(fcl::CollisionObjectd* o1, fcl::CollisionObjectd* o2, void
return false;
}
if (cdata->req->verbose)
ROS_DEBUG_NAMED("collision_detection.fcl", "Actually checking collisions between %s and %s", cd1->getID().c_str(),
RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Actually checking collisions between %s and %s", cd1->getID().c_str(),
cd2->getID().c_str());

fcl::DistanceResultd fcl_result;
Expand Down Expand Up @@ -674,15 +667,15 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
{
if (cache_it->second->collision_geometry_data_->ptr.raw == data)
{
// ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
// RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision data structures for object %s retrieved from
// cache.",
// cache_it->second->collision_geometry_data_->getID().c_str());
return cache_it->second;
}
else if (cache_it->second.unique())
{
const_cast<FCLGeometry*>(cache_it->second.get())->updateCollisionGeometryData(data, shape_index, false);
// ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for object %s retrieved from
// RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision data structures for object %s retrieved from
// cache after updating
// the source
// object.", cache_it->second->collision_geometry_data_->getID().c_str());
Expand Down Expand Up @@ -711,7 +704,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
// update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);

// ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for attached body %s retrieved
// RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision data structures for attached body %s retrieved
// from the cache for
// world objects.",
// obj_cache->collision_geometry_data_->getID().c_str());
Expand Down Expand Up @@ -745,7 +738,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
// update the CollisionGeometryData; nobody has a pointer to this, so we can safely modify it
const_cast<FCLGeometry*>(obj_cache.get())->updateCollisionGeometryData(data, shape_index, true);

// ROS_DEBUG_NAMED("collision_detection.fcl", "Collision data structures for world object %s retrieved
// RCLCPP_DEBUG(LOGGER_COLLISION_DETECTION, "Collision data structures for world object %s retrieved
// from the cache for
// attached
// bodies.",
Expand Down Expand Up @@ -823,7 +816,7 @@ FCLGeometryConstPtr createCollisionGeometry(const shapes::ShapeConstPtr& shape,
}
break;
default:
ROS_ERROR_NAMED("collision_detection.fcl", "This shape type (%d) is not supported using FCL yet",
RCLCPP_ERROR(LOGGER_COLLISION_DETECTION, "This shape type (%d) is not supported using FCL yet",
(int)shape->type);
cg_g = nullptr;
}
Expand Down
14 changes: 8 additions & 6 deletions moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,8 @@
#include <fcl/broadphase/broadphase_dynamic_AABB_tree.h>
#endif

rclcpp::Logger LOGGER_COLLISION_ROBOT_FCL = rclcpp::get_logger("collision_robot.fcl");
Copy link
Contributor

Choose a reason for hiding this comment

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

move inside the namespace

Copy link
Contributor

Choose a reason for hiding this comment

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

a3011f8


namespace collision_detection
{
CollisionRobotFCL::CollisionRobotFCL(const robot_model::RobotModelConstPtr& model, double padding, double scale)
Expand Down Expand Up @@ -68,7 +70,7 @@ CollisionRobotFCL::CollisionRobotFCL(const robot_model::RobotModelConstPtr& mode
fcl_objs_[index] = FCLCollisionObjectConstPtr(new fcl::CollisionObjectd(g->collision_geometry_));
}
else
ROS_ERROR_NAMED("collision_detection.fcl", "Unable to construct collision geometry for link '%s'",
RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_FCL, "Unable to construct collision geometry for link '%s'",
link->getName().c_str());
}
}
Expand Down Expand Up @@ -156,14 +158,14 @@ void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, Collisio
const robot_state::RobotState& state1,
const robot_state::RobotState& state2) const
{
ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_FCL, "FCL continuous collision checking not yet implemented");
}

void CollisionRobotFCL::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
const robot_state::RobotState& state1, const robot_state::RobotState& state2,
const AllowedCollisionMatrix& acm) const
{
ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_FCL, "FCL continuous collision checking not yet implemented");
}

void CollisionRobotFCL::checkSelfCollisionHelper(const CollisionRequest& req, CollisionResult& res,
Expand Down Expand Up @@ -209,7 +211,7 @@ void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, Collisi
const robot_state::RobotState& other_state1,
const robot_state::RobotState& other_state2) const
{
ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_FCL, "FCL continuous collision checking not yet implemented");
}

void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
Expand All @@ -219,7 +221,7 @@ void CollisionRobotFCL::checkOtherCollision(const CollisionRequest& req, Collisi
const robot_state::RobotState& other_state2,
const AllowedCollisionMatrix& acm) const
{
ROS_ERROR_NAMED("collision_detection.fcl", "FCL continuous collision checking not yet implemented");
RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_FCL, "FCL continuous collision checking not yet implemented");
}

void CollisionRobotFCL::checkOtherCollisionHelper(const CollisionRequest& req, CollisionResult& res,
Expand Down Expand Up @@ -274,7 +276,7 @@ void CollisionRobotFCL::updatedPaddingOrScaling(const std::vector<std::string>&
}
}
else
ROS_ERROR_NAMED("collision_detection.fcl", "Updating padding or scaling for unknown link: '%s'", link.c_str());
RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_FCL, "Updating padding or scaling for unknown link: '%s'", link.c_str());
}
}

Expand Down
Loading