Port occupancy_map_monitor to ROS2 #148

RoboticsYY · 2019-12-20T10:20:19Z

#112 is blocked by occupancy_map_monitor, please see https://github.com/ros-planning/moveit2/blob/fa81678b965eafcb1e8c72018ba4d99ab2593255/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h#L45

This work followed AcutronicRobotics' porting, but didn't use the original commits. Because occupancy_map_monitor was moved from moveit_ros to moveit_ros_perception(ref), the original commits are very difficult to directly use.

JafarAbdi

Thank you @RoboticsYY for porting occupancy_map_monitor

JafarAbdi · 2019-12-20T13:34:45Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt

  src/occupancy_map_monitor.cpp
  src/occupancy_map_updater.cpp
  )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core moveit_msgs pluginlib octomap geometric_shapes)
+target_link_libraries(${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})


Suggested change

target_link_libraries(${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})

Please add Boost in ament_target_dependencies

Done as suggested.

JafarAbdi · 2019-12-20T13:38:28Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt

  src/occupancy_map_monitor.cpp
  src/occupancy_map_updater.cpp
  )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core moveit_msgs pluginlib octomap geometric_shapes)


Suggested change

ament_target_dependencies(${MOVEIT_LIB_NAME} rclcpp moveit_core moveit_msgs pluginlib octomap geometric_shapes)

ament_target_dependencies(${MOVEIT_LIB_NAME}

rclcpp

moveit_core

moveit_msgs

pluginlib

octomap

geometric_shapes

Boost)

Done as suggested.

JafarAbdi · 2019-12-20T13:39:23Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt


 add_executable(moveit_ros_occupancy_map_server src/occupancy_map_server.cpp)
-target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(moveit_ros_occupancy_map_server rclcpp moveit_core moveit_msgs pluginlib octomap geometric_shapes)
+target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})


Suggested change

target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})

target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME})

Same

Done adding Boost to ament_target_dependencies.

JafarAbdi · 2019-12-20T13:39:59Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt

+ament_export_dependencies(pluginlib)
+ament_export_dependencies(octomap)
+ament_export_dependencies(geometric_shapes)
+


Please add ament_export_dependencies(Boost)

Added Boost to ament_export_dependencies.

JafarAbdi · 2019-12-20T13:40:31Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt

 )

 install(TARGETS moveit_ros_occupancy_map_server
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  RUNTIME DESTINATION share/${PROJECT_NAME}/


Shouldn't this be lib/${PROJECT_NAME} .?

This has been corrected.

JafarAbdi · 2019-12-20T13:41:45Z

moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h

-                      double map_resolution = 0.0);
-  OccupancyMapMonitor(double map_resolution = 0.0);
-  OccupancyMapMonitor(const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, ros::NodeHandle& nh,
+  OccupancyMapMonitor(const std::shared_ptr<tf2_ros::Buffer>& tf_buffer, const rclcpp::Node::SharedPtr& node,


For consistency node should be the first parameter

Done as suggested.

JafarAbdi · 2019-12-20T13:44:17Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp

-{
-  initialize();
-}
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_monitor");


Suggested change

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_monitor");

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_occupancy_map_monitor");

Nit: @henningkayser Which one should we use .?

Both are fine, I would stick with the full lib name as we decided to do so and follow this rule in other packages.
My general concern is that this won't scale with added files and also the namespaces are less explicit (i.e. for scoping moveit, moveit_ros, etc).

JafarAbdi · 2019-12-20T13:46:31Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp


-static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor* server)
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_server");


Suggested change

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_server");

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_occupancy_map_server");

Nit: @henningkayser Which one should we use .?

Ok, now that we have multiple files in this package I would strongly prefer lib_name.file_name for all files. It's better to be more explicit about this and makes scoping logger levels easier

JafarAbdi · 2019-12-20T13:48:15Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp

@@ -39,6 +39,8 @@

 namespace occupancy_map_monitor
 {
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_updater");


Suggested change

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_updater");

static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_occupancy_map_updater");

Nit: @henningkayser Which one should we use .?

JafarAbdi · 2019-12-20T13:56:43Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp

 {
  transform_cache_.clear();
  if (transform_provider_callback_)
    return transform_provider_callback_(target_frame, target_time, transform_cache_);
  else
  {
-    ROS_WARN_THROTTLE(1, "No callback provided for updating the transform cache for octomap updaters");
+    RCUTILS_LOG_ERROR_THROTTLE(rcutils_steady_time_now, 1,


Should we use RCUTILS* directly .?

No, we should use RCLCPP_WARN_THROTTLE which becomes available with Eloquent. So this is on me ;)

henningkayser · 2019-12-20T15:36:37Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt

 project(moveit_ros_occupancy_map_monitor)
 set(MOVEIT_LIB_NAME ${PROJECT_NAME})

-set(CMAKE_CXX_STANDARD 14)
+# Default to C++14


I think we should keep the original cmake flags and warnings and modify them globally if necessary

Changed back to the original setting.

henningkayser · 2019-12-20T15:37:19Z

moveit_ros/occupancy_map_monitor/CMakeLists.txt


 include_directories(include
                    ${Boost_INCLUDE_DIRS}
-                    ${catkin_INCLUDE_DIRS}
+                    ${rclcpp_INCLUDE_DIRS}


This shouldn't be necessary with ament_target_dependencies

Removed ${rclcpp_INCLUDE_DIRS} from include_directories.

henningkayser · 2019-12-20T15:44:20Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp

-{
-  initialize();
-}
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_monitor");


Both are fine, I would stick with the full lib name as we decided to do so and follow this rule in other packages.
My general concern is that this won't scale with added files and also the namespaces are less explicit (i.e. for scoping moveit, moveit_ros, etc).

henningkayser · 2019-12-20T15:44:53Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp

-    }
-  }
+  // TODO rework this in ROS2
+  // XmlRpc::XmlRpcValue sensor_list;


@JafarAbdi Didn't you have a code snippet where you already replaced XmlRPC ?

It's a work-around the way we specify the current parameters, for example for sensor_3d.yaml it'll be

/**: ros__parameters: sensors: ["sensor1", "sensor2"] sensor1: sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater point_cloud_topic: /head_mount_kinect/depth_registered/points max_range: 5.0 point_subsample: 1 padding_offset: 0.1 padding_scale: 1.0 max_update_rate: 1.0 filtered_cloud_topic: filtered_cloud sensor2: sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater image_topic: /head_mount_kinect/depth_registered/image_raw queue_size: 5 near_clipping_plane_distance: 0.3 far_clipping_plane_distance: 5.0 shadow_threshold: 0.2 padding_scale: 4.0 padding_offset: 0.03 max_update_rate: 1.0 filtered_cloud_topic: filtered_cloud

This link inspired me to do so

@henningkayser Does this method makes sense .? if you agree with this method I'll open PR fixes it targeting this branch

I think it seems a little redundant to have the sensor names extra, but if it makes things easier I agree with this approach.

However, I think this belongs to a system wide launch/parameter refactoring, so I would suggest we merge this right away to continue with PSM

Yes, I agree with the codebase wide refactoring for XmlRpc

moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp

henningkayser · 2019-12-20T15:48:29Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp


-static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor* server)
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_server");


Ok, now that we have multiple files in this package I would strongly prefer lib_name.file_name for all files. It's better to be more explicit about this and makes scoping logger levels easier

henningkayser · 2019-12-20T15:57:36Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp

 {
  transform_cache_.clear();
  if (transform_provider_callback_)
    return transform_provider_callback_(target_frame, target_time, transform_cache_);
  else
  {
-    ROS_WARN_THROTTLE(1, "No callback provided for updating the transform cache for octomap updaters");
+    RCUTILS_LOG_ERROR_THROTTLE(rcutils_steady_time_now, 1,


No, we should use RCLCPP_WARN_THROTTLE which becomes available with Eloquent. So this is on me ;)

RoboticsYY · 2019-12-24T10:53:22Z

@JafarAbdi @henningkayser All the issues are addressed except replacing XmlRPC and changing ROS_WARN_THROTTLE to RCLCPP_WARN_THROTTLE.

henningkayser · 2020-01-17T12:49:23Z

moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp

-    }
-  }
+  // TODO rework this in ROS2
+  // XmlRpc::XmlRpcValue sensor_list;


However, I think this belongs to a system wide launch/parameter refactoring, so I would suggest we merge this right away to continue with PSM

RoboticsYY force-pushed the pr-occupancy_map_monitor branch from ea8e8bb to 955e367 Compare December 20, 2019 10:26

JafarAbdi requested changes Dec 20, 2019

View reviewed changes

henningkayser reviewed Dec 20, 2019

View reviewed changes

JafarAbdi mentioned this pull request Dec 20, 2019

Fixing docker #150

Merged

6 tasks

RoboticsYY force-pushed the pr-occupancy_map_monitor branch 5 times, most recently from 14b53c1 to 6aed2b3 Compare December 24, 2019 10:39

RoboticsYY force-pushed the pr-occupancy_map_monitor branch from 6aed2b3 to c4a7c93 Compare December 25, 2019 03:01

RoboticsYY added 4 commits January 15, 2020 10:41

Enable build of occupancy_map_monitor

83f4df5

Port occupancy_map_updater

ba3eac5

Port occupancy_map_monitor

7a5ef3d

Port occupancy_map_server

982ece5

RoboticsYY force-pushed the pr-occupancy_map_monitor branch from c4a7c93 to 982ece5 Compare January 15, 2020 02:42

Using THROTTLE macros

1ff5ead

RoboticsYY force-pushed the pr-occupancy_map_monitor branch from 352e4b7 to 1ff5ead Compare January 15, 2020 07:26

henningkayser approved these changes Jan 17, 2020

View reviewed changes

henningkayser merged commit defd82f into moveit:master Jan 17, 2020

JafarAbdi mentioned this pull request Jan 17, 2020

Revise XML-RPC parameter lookup #155

Closed

henningkayser mentioned this pull request May 20, 2020

Replace XmlRpc parameter parsing #197

Closed

RoboticsYY mentioned this pull request Dec 14, 2020

Porting moveit_ros_perception/pointcloud_octomap_updater #307

Merged

6 tasks

davetcoleman mentioned this pull request Sep 22, 2021

Update Maintainers of MoveIt package #697

Merged

	target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME} ${Boost_LIBRARIES})
	target_link_libraries(moveit_ros_occupancy_map_server ${MOVEIT_LIB_NAME})

	static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_monitor");
	static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros_occupancy_map_monitor");


		static void publishOctomap(ros::Publisher* octree_binary_pub, occupancy_map_monitor::OccupancyMapMonitor* server)
		static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.occupancy_map_server");

Port occupancy_map_monitor to ROS2 #148

Port occupancy_map_monitor to ROS2 #148

Conversation

RoboticsYY commented Dec 20, 2019

JafarAbdi left a 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

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

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

RoboticsYY commented Dec 24, 2019

Choose a reason for hiding this comment