Port collision_detection_fcl to ROS 2 #41

vmayoral · 2019-03-10T17:11:30Z

Pending:

Port unit tests

@anasarrak

mlautman · 2019-03-11T22:50:28Z

..._detection_fcl/include/moveit/collision_detection_fcl/collision_detector_fcl_plugin_loader.h

@@ -0,0 +1,19 @@
+/*


This file should be available by cherry picking in moveit/moveit@d8a9337. I don't think it is a good idea to copy-paste in this file without the rest of the PR?

@BryceStevenWilley can you comment on this?

Agreed, cherry picking that PR should be easier. This PR is missing the plugin XML, and the modification to the package.xml. FCL will still load by default, but things might break when you try to manually load the plugin. However, I can't speak to how pluginlib works in ROS2/if any of these files need to be modified.

@vmayoral As your team tackles pluginlib in ros2, it may be helpful to have examples to draw from. RQT2 and RVIZ2 both use pluginlib.

When I was porting the CPP plugin provider for RQT2 this README in the rviz2 repo was really useful: https://github.com/ros2/rviz/blob/ros2/docs/plugin_development.md#plugin-basics

rqt_gui_cpp in the rqt repo uses the new pluginlib for ros2. It may be helpful to look at the diff between crystal-devel and kinetic-devel and reference the changes to rqt_gui_cpp.

rqt_image_view is another good example of a plugin that gets registered and loaded by pluginlib:
branch
(Diff between master and crystal-devel)

Thanks @mlautman having look now. Experiencing some issues with rviz2 though ros2/rviz#385 in OS X.

moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h

...it_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_robot_fcl.h

...it_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_world_fcl.h

henningkayser

I'll approve with this minor refactoring to use const rclcpp::Logger LOGGER.

moveit_core/collision_detection_fcl/include/moveit/collision_detection_fcl/collision_common.h

henningkayser · 2019-04-09T17:15:55Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

@@ -50,6 +50,8 @@
 #include <boost/thread/mutex.hpp>
 #include <memory>

+rclcpp::Logger logger = rclcpp::get_logger("collision_detection.fcl");


Suggested change

rclcpp::Logger logger = rclcpp::get_logger("collision_detection.fcl");

rclcpp::Logger LOGGER = rclcpp::get_logger("collision_detection.fcl");

The issue has been addressed on the following #41 (comment)

henningkayser · 2019-04-09T17:16:06Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

@@ -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 between '%s' (type '%s') and '%s' (type '%s') is always allowed. "


Suggested change

RCLCPP_DEBUG(logger, "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "

RCLCPP_DEBUG(LOGGER, "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "

We have changed the logger name, this should be fixed a3011f8

moveit_core/collision_detection_fcl/src/collision_common.cpp

moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp

henningkayser · 2019-04-09T17:21:32Z

moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp

@@ -52,6 +52,8 @@

 #include <boost/bind.hpp>

+rclcpp::Logger logger = rclcpp::get_logger("collision_detection.fcl");


Suggested change

rclcpp::Logger logger = rclcpp::get_logger("collision_detection.fcl");

rclcpp::Logger LOGGER = rclcpp::get_logger("collision_detection.fcl");

We have changed the logger name, this should be fixed a3011f8

moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp

….txt

moveit_core/collision_detection_fcl/CMakeLists.txt

…etection_fcl/collision_common.h Thanks Co-Authored-By: Henning Kayser <henningkayser@picknik.ai>

moveit_core/collision_detection_fcl/CMakeLists.txt

From https://github.com/ros-planning/moveit2/pull/41/files/195e3198435637f4b540860190543b2c17ba4313#r284908197

mlautman · 2019-05-21T21:15:17Z

moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp

 namespace collision_detection
 {
-const std::string CollisionDetectorAllocatorFCL::NAME("FCL");
+const std::string CollisionDetectorAllocatorFCL::NAME_("FCL");


This has to match CollisionDetectorAllocatorTemplate::NAME please revert

mlautman · 2019-05-21T21:17:58Z

...sion_detection_fcl/include/moveit/collision_detection_fcl/collision_detector_allocator_fcl.h

@@ -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


mlautman · 2019-05-21T21:18:25Z

moveit_core/collision_detection_fcl/src/collision_common.cpp

@@ -50,6 +50,8 @@
 #include <boost/thread/mutex.hpp>
 #include <memory>

+rclcpp::Logger LOGGER_COLLISION_DETECTION = rclcpp::get_logger("collision_detection.fcl");


move inside the namespace

mlautman · 2019-05-21T21:20:25Z

moveit_core/collision_detection_fcl/src/collision_robot_fcl.cpp

@@ -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");


move inside the namespace

mlautman · 2019-05-21T21:20:42Z

moveit_core/collision_detection_fcl/src/collision_world_fcl.cpp

@@ -52,9 +52,11 @@

 #include <boost/bind.hpp>

+rclcpp::Logger LOGGER_COLLISION_WORLD = rclcpp::get_logger("collision_world.fcl");


move inside the namespace

ahcorde · 2019-05-24T11:06:26Z

@mlautman Updated

mlautman · 2019-05-29T19:26:37Z

moveit_core/collision_detection_fcl/CMakeLists.txt

+  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")


Hardcoding relative paths is a bad idea.

I would recommend that for every library in this package that is used else ware you set a <lib_name>_lib_dir environment variable that can then be used by the tests

A good example is the rcl_lib_dir I have taken the time to show how it is used below:

/home/mike/ws_ros2/src/ros2/rcl/rcl/CMakeLists.txt: 78 79: # rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so 80 # the librcl that they link against is on the library path. 81 # This is especially important on Windows. 82 # This is overwritten each loop, but which one it points to doesn't really matter. 83: set(rcl_lib_dir "$<TARGET_FILE_DIR:${PROJECT_NAME}>") 84 /home/mike/ws_ros2/src/ros2/rcl/rcl/test/CMakeLists.txt: 16 17: set(extra_lib_dirs "${rcl_lib_dir}") 18 /home/mike/ws_ros2/src/ros2/rcl/rcl/test/CMakeLists.txt: 34 rcl_add_custom_gtest(test_client${target_suffix} 35 SRCS rcl/test_client.cpp 36: INCLUDE_DIRS ${osrf_testing_tools_cpp_INCLUDE_DIRS} 37 ENV ${rmw_implementation_env_var} 38 APPEND_LIBRARY_DIRS ${extra_lib_dirs}

The example uses rcl_add_custom_gtest but it works just as well with ament_add_gtest

If you don't mind I prefer to merge this. And then I will fix them all together. Because we have other merged that use this style. I can open an issue to avoid forgetting it.

friendly ping @mlautman

mlautman

See comment about CMakeLists.txt I approve once the feedback has been addressed.

moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp

anasarrak · 2019-06-03T09:49:16Z

The remaining issues are already addresed, can we merge this? @mlautman @davetcoleman

mlautman · 2019-06-03T22:59:11Z

@anasarrak This is still failing CI. Please investigate why that is and get things passing. Then I will merge

anasarrak · 2019-06-05T08:16:56Z

@mlautman As @LanderU mentioned on the following issue #92, the CI is not passing because of the latest update on the keys.

anasarrak · 2019-06-06T09:28:40Z

Ping @mlautman @davetcoleman @henningkayser

Port collision_detection_fcl to ROS 2

90fc0cb

Authored by @anasarrak

mlautman reviewed Mar 11, 2019

View reviewed changes

henningkayser reviewed Mar 18, 2019

View reviewed changes

vmayoral mentioned this pull request Mar 20, 2019

sync MoveIt 1 master #47

Merged

vmayoral added 4 commits March 30, 2019 19:44

collision_detection_fcl, fix CMakeLists.txt

d4e505e

Merge branch 'moveit2' into collision-detection-fcl

5a169f5

xcollision_detector_fcl, missing dependencies

615642d

collision_detection_fcl, remove leftovers from logger prototype

534f026

vmayoral mentioned this pull request Apr 8, 2019

Planning interface, port to ROS 2 #61

Merged

henningkayser reviewed Apr 9, 2019

View reviewed changes

ahcorde added 4 commits May 16, 2019 16:03

collision_detection_fcl - renamed NAME to NAME_

ffaf3f5

Collision_detection_fcl - fixing logger variables name

183c9a9

collision_detection_fcl - porting test_fcl_collision_detection.cpp

17b79ef

collision_detection_fcl - adding tests and dependencies to CMakeLists…

195e319

….txt

davetcoleman approved these changes May 16, 2019

View reviewed changes

moveit_core/collision_detection_fcl/CMakeLists.txt Outdated Show resolved Hide resolved

Update moveit_core/collision_detection_fcl/include/moveit/collision_d…

efbbfd6

…etection_fcl/collision_common.h Thanks Co-Authored-By: Henning Kayser <henningkayser@picknik.ai>

vmayoral commented May 16, 2019

View reviewed changes

moveit_core/collision_detection_fcl/CMakeLists.txt Outdated Show resolved Hide resolved

Víctor Mayoral Vilches and others added 3 commits May 16, 2019 23:48

Update moveit_core/collision_detection_fcl/CMakeLists.txt

fbd492f

From https://github.com/ros-planning/moveit2/pull/41/files/195e3198435637f4b540860190543b2c17ba4313#r284908197

clang

d1c729b

tabs to spaces

265675c

mlautman reviewed May 21, 2019

View reviewed changes

ahcorde added 2 commits May 24, 2019 13:02

moveit_core collision detection_fcl - loggers inside namespace

a3011f8

moveit_core collision_detection_fcl - revert NAME_ to NAME

18a607d

ahcorde mentioned this pull request May 24, 2019

Moveit core CMakeLists.txt #90

Closed

14 tasks

mlautman reviewed May 29, 2019

View reviewed changes

davetcoleman reviewed May 31, 2019

View reviewed changes

moveit_core/collision_detection_fcl/test/test_fcl_collision_detection.cpp Outdated Show resolved Hide resolved

anasarrak mentioned this pull request Jun 3, 2019

Fix fcl_collision_detection_test AcutronicRobotics/moveit2#99

Closed

Fix collision_detection logger

ed4f512

anasarrak added 4 commits June 3, 2019 12:23

Fix fix_fcl_collision_detection

cbc0b48

move logger collision_common

86c1614

Revert back logger

17688f0

clang format

992fef0

mlautman merged commit f4601e5 into moveit:master Jun 12, 2019

ahcorde deleted the collision-detection-fcl branch June 12, 2019 21:54

MikeWrock pushed a commit to MikeWrock/moveit2 that referenced this pull request Aug 15, 2022

consistency for code blocks (moveit#41)

b8fae26

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Port collision_detection_fcl to ROS 2 #41

Port collision_detection_fcl to ROS 2 #41

vmayoral commented Mar 10, 2019 •

edited

Loading

mlautman Mar 11, 2019

BryceStevenWilley Mar 11, 2019

mlautman Mar 12, 2019

vmayoral Mar 17, 2019

henningkayser left a comment

henningkayser Apr 9, 2019 •

edited

Loading

anasarrak Jun 6, 2019

henningkayser Apr 9, 2019

anasarrak Jun 6, 2019

henningkayser Apr 9, 2019 •

edited

Loading

anasarrak Jun 6, 2019

mlautman May 21, 2019

anasarrak Jun 3, 2019

mlautman May 21, 2019

anasarrak Jun 3, 2019

mlautman May 21, 2019

anasarrak Jun 3, 2019

mlautman May 21, 2019

anasarrak Jun 3, 2019

mlautman May 21, 2019

anasarrak Jun 3, 2019

ahcorde commented May 24, 2019

mlautman May 29, 2019

ahcorde May 29, 2019

ahcorde May 30, 2019

mlautman left a comment

anasarrak commented Jun 3, 2019

mlautman commented Jun 3, 2019

anasarrak commented Jun 5, 2019

anasarrak commented Jun 6, 2019

	rclcpp::Logger logger = rclcpp::get_logger("collision_detection.fcl");
	rclcpp::Logger LOGGER = rclcpp::get_logger("collision_detection.fcl");

	RCLCPP_DEBUG(logger, "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "
	RCLCPP_DEBUG(LOGGER, "Collision between '%s' (type '%s') and '%s' (type '%s') is always allowed. "

		@@ -52,6 +52,8 @@

		#include <boost/bind.hpp>

		rclcpp::Logger logger = rclcpp::get_logger("collision_detection.fcl");

		@@ -52,9 +52,11 @@

		#include <boost/bind.hpp>

		rclcpp::Logger LOGGER_COLLISION_WORLD = rclcpp::get_logger("collision_world.fcl");

Port collision_detection_fcl to ROS 2 #41

Port collision_detection_fcl to ROS 2 #41

Conversation

vmayoral commented Mar 10, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

henningkayser left a comment

Choose a reason for hiding this comment

henningkayser Apr 9, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

henningkayser Apr 9, 2019 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ahcorde commented May 24, 2019

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mlautman left a comment

Choose a reason for hiding this comment

anasarrak commented Jun 3, 2019

mlautman commented Jun 3, 2019

anasarrak commented Jun 5, 2019

anasarrak commented Jun 6, 2019

vmayoral commented Mar 10, 2019 •

edited

Loading

henningkayser Apr 9, 2019 •

edited

Loading

henningkayser Apr 9, 2019 •

edited

Loading