Port collision_detection to ROS2 #40

vmayoral · 2019-03-10T17:07:03Z

No description provided.

moveit_core/collision_detection/include/moveit/collision_detection/collision_log.h

moveit_core/collision_detection/src/collision_tools.cpp

Co-Authored-By: anasarrak <anasarrak@gmail.com>

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

moveit_core/collision_detection/src/collision_matrix.cpp

moveit_core/collision_detection/src/collision_tools.cpp

mlautman

Please address comments regarding 1) const, 2) logger, and 3) clang-format

Remove also dedicated log.h file.

vmayoral · 2019-03-17T08:06:15Z

/Users/victor/ros2_moveit_ws/src/moveit2/moveit_core/collision_detection/src/collision_tools.cpp:281:14: error: no viable overloaded '='
  msg.normal = tf2::toMsg(contact.normal);
  ~~~~~~~~~~ ^ ~~~~~~~~~~~~~~~~~~~~~~~~~~
/Users/victor/ros2_ws/install/geometry_msgs/include/geometry_msgs/msg/vector3__struct.hpp:37:8: note: candidate function (the implicit copy assignment operator) not viable: no known conversion from 'geometry_msgs::msg::Point' (aka 'Point_<std::allocator<void> >') to 'const geometry_msgs::msg::Vector3_<std::__1::allocator<void> >' for 1st argument
struct Vector3_
       ^
/Users/victor/ros2_ws/install/geometry_msgs/include/geometry_msgs/msg/vector3__struct.hpp:37:8: note: candidate function (the implicit move assignment operator) not viable: no known conversion from 'geometry_msgs::msg::Point' (aka 'Point_<std::allocator<void> >') to 'geometry_msgs::msg::Vector3_<std::__1::allocator<void> >' for 1st argument
2 warnings and 1 error generated.

Looking at the messages:

victor:~/ros2_ws/src/ros2/common_interfaces/geometry_msgs/msg ((HEAD detached at 0.6.1))$
| => cat Point.msg
# This contains the position of a point in free space
float64 x
float64 y
float64 z
________________________________________________________________________________
| victor:~/ros2_ws/src/ros2/common_interfaces/geometry_msgs/msg ((HEAD detached at 0.6.1))$
| => cat Vector3.msg
# This represents a vector in free space.

float64 x
float64 y
float64 z

Addressed with AcutronicRobotics/geometry2@a5dfb74. @wjwwood please advise if this is the desired way. Probably creating a toMsg2 isn't what you've got in mind?

With the changes above in geometry2, submodule now compiles. Ready for your review @mlautman.

Convert a Eigen::Vector3d type to a geometry_msgs::msg::Vector3

wjwwood · 2019-03-18T15:14:43Z

I have no idea, it would be a better question for @tfoote maybe.

henningkayser

Looks good to me. I would just replace the clock references by static rclcpp::Time::now() calls.

moveit_core/collision_detection/src/collision_tools.cpp

Co-Authored-By: vmayoral <victor@erlerobotics.com>

tfoote · 2019-03-19T00:23:04Z

Addressed with AcutronicRobotics/geometry2@a5dfb74. @wjwwood please advise if this is the desired way. Probably creating a toMsg2 isn't what you've got in mind?

With the changes above in geometry2, submodule now compiles. Ready for your review @mlautman.

Point and Vector are definitely different types and shouldn't have implicit conversions. Can you specify which template arguments to use for the toMsg to resolve this?

vmayoral · 2019-03-30T10:59:25Z

Point and Vector are definitely different types and shouldn't have implicit conversions. Can you specify which template arguments to use for the toMsg to resolve this?

@tfoote it's been a few days since I last looked at this so let me see if I remember everything:

AFAIK, the template I was trying to use was https://github.com/AcutronicRobotics/geometry2/blob/a5dfb7424c972d06b3f17cbad2dfa05af95163b6/tf2/include/tf2/convert.h#L93 with the following form: geometry_msgs::msg::Vector3 toMsg(const Eigen::Vector3d& in) however
there already existed a https://github.com/AcutronicRobotics/geometry2/blob/a5dfb7424c972d06b3f17cbad2dfa05af95163b6/tf2_eigen/include/tf2_eigen/tf2_eigen.h#L130 and it simply wasn't possible to overload the function with a different return type thereby
I came up with AcutronicRobotics/geometry2@a5dfb74#diff-f5c2ddfab4a88cc4877766f5d49523deR364

Not sure if this answers your question though. Let me know if you require further changes on this PR or if you're fine with them and we can proceed here.

vmayoral · 2019-03-30T17:19:48Z

Reworked the module according to the discussions held regarding logging. Unit tests are disabled due to linking issues, similar than the ones described at AcutronicRobotics#13

henningkayser

Reworked the module according to the discussions held regarding logging. Unit tests are disabled due to linking issues, similar than the ones described at AcutronicRobotics#13

Looks good in general. I would just suggest using const loggers and fixing the rclcpp::Duration constructors.

moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp

moveit_core/collision_detection/src/allvalid/collision_world_allvalid.cpp

moveit_core/collision_detection/src/collision_matrix.cpp

moveit_core/collision_detection/src/collision_octomap_filter.cpp

moveit_core/collision_detection/src/collision_robot.cpp

henningkayser · 2019-04-09T17:05:29Z

moveit_core/collision_detection/src/collision_tools.cpp

  color.r = 1.0f;
  color.g = 0.5f;
  color.b = 0.0f;
  color.a = 0.4f;
-  getCostMarkers(arr, frame_id, cost_sources, color, ros::Duration(60.0));
+  getCostMarkers(arr, frame_id, cost_sources, color, rclcpp::Duration(60.0));


Suggested change

getCostMarkers(arr, frame_id, cost_sources, color, rclcpp::Duration(60.0));

getCostMarkers(arr, frame_id, cost_sources, color, rclcpp::Duration(60, 0));

rclcpp::Duration doesn't support the seconds constructor anymore.

henningkayser · 2019-04-09T17:05:39Z

moveit_core/collision_detection/src/collision_tools.cpp

  color.r = 1.0f;
  color.g = 0.0f;
  color.b = 0.0f;
  color.a = 0.8f;
-  getCollisionMarkersFromContacts(arr, frame_id, con, color, ros::Duration(60.0));
+  getCollisionMarkersFromContacts(arr, frame_id, con, color, rclcpp::Duration(60.0));


henningkayser · 2019-04-09T17:08:20Z

moveit_core/collision_detection/src/collision_tools.cpp

 {
  int id = 0;
  for (const auto& cost_source : cost_sources)
  {
-    visualization_msgs::Marker mk;
-    mk.header.stamp = ros::Time::now();
+    rclcpp::Clock ros_clock(RCL_ROS_TIME);


For what do we need ros_clock here? It could probably be used to initialize mk.header.stamp below by calling ros_clock.now() like already implemented in the changes below.

moveit_core/collision_detection/src/world.cpp

moveit_core/collision_detection/CMakeLists.txt

davetcoleman · 2019-05-16T21:32:19Z

moveit_core/collision_detection/CMakeLists.txt

+if(BUILD_TESTING)
+	find_package(resource_retriever REQUIRED)
+	if(WIN32)
+		# set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")


Add TODO comment about why this is not working, or remove

moveit_core/collision_detection/CMakeLists.txt

moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp

…otics/moveit2 into collision-detection

ahcorde · 2019-05-24T10:56:46Z

Fixed indentation in the CMakeLists.txt and removed duplicate ;

mlautman · 2019-05-29T19:28:18Z

moveit_core/collision_detection/CMakeLists.txt

+  if(WIN32)
+    # TODO add window paths
+  else()
+   set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${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.

Please open an issue

Opened one #91

mlautman · 2019-05-29T19:31:21Z

@ahcorde See remaining comments from Henning, Dave and myself

ahcorde · 2019-05-31T16:14:08Z

@mlautman @henningkayser @davetcoleman Ready to merge

addressed

…_acu Port Planning pipeline

Port collision_detection to ROS2

49efb93

vmayoral commented Mar 10, 2019

View reviewed changes

moveit_core/collision_detection/include/moveit/collision_detection/collision_log.h Outdated Show resolved Hide resolved

vmayoral commented Mar 10, 2019

View reviewed changes

moveit_core/collision_detection/src/collision_tools.cpp Outdated Show resolved Hide resolved

Update moveit_core/collision_detection/src/collision_tools.cpp

376cd4f

Co-Authored-By: anasarrak <anasarrak@gmail.com>