moveit · henningkayser · Jan 31, 2020 · Jan 31, 2020 · Jan 31, 2020 · Jan 31, 2020
diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt
@@ -3,6 +3,7 @@ set(MOVEIT_LIB_NAME moveit_utils)
 add_library(${MOVEIT_LIB_NAME} SHARED
   src/lexical_casts.cpp
   src/message_checks.cpp
+  src/rclcpp_utils.cpp
 )
 
 ament_target_dependencies(${MOVEIT_LIB_NAME} Boost moveit_msgs)

diff --git a/moveit_core/utils/include/moveit/utils/rclcpp_utils.h b/moveit_core/utils/include/moveit/utils/rclcpp_utils.h
@@ -0,0 +1,44 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the names of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef ROSCPP_NAMES_H
+#define ROSCPP_NAMES_H
+
+#include <string>
+
+// TODO(JafarAbdi): This's taken from ros_comm/../names.cpp for MoveIt 2 beta release -- remove when it's ported
+namespace rclcpp
+{
+namespace names
+{
+std::string clean(const std::string& name);
+
+std::string append(const std::string& left, const std::string& right);
+}  // namespace names
+}  // namespace rclcpp
+
+#endif  // ROSCPP_NAMES_H
diff --git a/moveit_core/utils/src/rclcpp_utils.cpp b/moveit_core/utils/src/rclcpp_utils.cpp
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2009, Willow Garage, Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <moveit/utils/rclcpp_utils.h>
+
+namespace rclcpp
+{
+namespace names
+{
+std::string clean(const std::string& name)
+{
+  std::string clean = name;
+
+  size_t pos = clean.find("//");
+  while (pos != std::string::npos)
+  {
+    clean.erase(pos, 1);
+    pos = clean.find("//", pos);
+  }
+
+  if (!name.empty() && *clean.rbegin() == '/')
+  {
+    clean.erase(clean.size() - 1, 1);
+  }
+
+  return clean;
+}
+
+std::string append(const std::string& left, const std::string& right)
+{
+  return clean(left + "/" + right);
+}
+}  // namespace names
+}  // namespace rclcpp
diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt
@@ -54,6 +54,17 @@ set(libraries
 
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
+  Boost
+  pluginlib
+  rclcpp
+  message_filters
+  srdfdom
+  urdf
+  tf2
+  tf2_eigen
+  tf2_ros
+  Eigen3
+  moveit_ros_occupancy_map_monitor
   moveit_core
   # moveit_ros_perception
   moveit_msgs

diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt
@@ -17,103 +17,77 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
 endif()
 
 find_package(Boost REQUIRED thread date_time system filesystem)
-find_package(catkin REQUIRED COMPONENTS
-  class_loader
-  geometric_shapes
-  interactive_markers
-  moveit_ros_perception
-  moveit_ros_planning_interface
-  moveit_ros_robot_interaction
-  moveit_ros_warehouse
-  object_recognition_msgs
-  pluginlib
-  rosconsole
-  roscpp
-  rospy
-  rviz
-  tf2_eigen
-  roscpp
-  rosconsole
-  object_recognition_msgs
-)
-# TODO: Remove when Kinetic support is dropped
-if(rviz_VERSION VERSION_LESS 1.13.1) # Does rviz supports TF2?
-  add_definitions(-DRVIZ_TF1)
-endif()
-
+find_package(ament_cmake REQUIRED)
+find_package(class_loader REQUIRED)
+find_package(geometric_shapes REQUIRED)
+find_package(interactive_markers REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(octomap_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+# TODO(JafarAbdi): Uncomment when porting each package
+# find_package(moveit_ros_perception REQUIRED)
+# find_package(moveit_ros_robot_interaction REQUIRED)
+# find_package(moveit_ros_warehouse REQUIRED)
+find_package(object_recognition_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rviz2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(rviz_ogre_vendor REQUIRED)
 
 # Qt Stuff
-if(rviz_QT_VERSION VERSION_LESS "5")
-	find_package(Qt4 ${rviz_QT_VERSION} REQUIRED QtCore QtGui)
-	include(${QT_USE_FILE})
-  macro(qt_wrap_ui)
-    qt4_wrap_ui(${ARGN})
-  endmacro()
-else()
-	find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets)
-	set(QT_LIBRARIES Qt5::Widgets)
-  macro(qt_wrap_ui)
-    qt5_wrap_ui(${ARGN})
-  endmacro()
-endif()
+find_package(Qt5 REQUIRED COMPONENTS Core Widgets)
 
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 set(CMAKE_AUTOMOC ON)
 add_definitions(-DQT_NO_KEYWORDS)
 
-catkin_package(
-  LIBRARIES
-    moveit_motion_planning_rviz_plugin_core
-    moveit_planning_scene_rviz_plugin_core
-    moveit_robot_state_rviz_plugin_core
-    moveit_rviz_plugin_render_tools
-    moveit_trajectory_rviz_plugin_core
-  INCLUDE_DIRS
-    motion_planning_rviz_plugin/include
-    planning_scene_rviz_plugin/include
-    robot_state_rviz_plugin/include
-    rviz_plugin_render_tools/include
-    trajectory_rviz_plugin/include
-  CATKIN_DEPENDS
-    moveit_ros_planning_interface
-    moveit_ros_robot_interaction
-    object_recognition_msgs
-  DEPENDS
-    EIGEN3
-)
-
-catkin_install_python(PROGRAMS scripts/moveit_joy.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
-catkin_python_setup()
+# catkin_install_python(PROGRAMS scripts/moveit_joy.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+# catkin_python_setup()
 
 include_directories(rviz_plugin_render_tools/include
                     robot_state_rviz_plugin/include
                     planning_scene_rviz_plugin/include
-                    motion_planning_rviz_plugin/include
-                    trajectory_rviz_plugin/include
-                    ${Boost_INCLUDE_DIRS}
-                    ${catkin_INCLUDE_DIRS})
-
-include_directories(SYSTEM
-                    ${EIGEN3_INCLUDE_DIRS}
-                    ${QT_INCLUDE_DIR})
+                    #motion_planning_rviz_plugin/include
+                    #trajectory_rviz_plugin/include
+)
 
 add_subdirectory(rviz_plugin_render_tools)
 add_subdirectory(robot_state_rviz_plugin)
 add_subdirectory(planning_scene_rviz_plugin)
-add_subdirectory(motion_planning_rviz_plugin)
-add_subdirectory(trajectory_rviz_plugin)
-
-install(FILES
-  motion_planning_rviz_plugin_description.xml
-  trajectory_rviz_plugin_description.xml
-  planning_scene_rviz_plugin_description.xml
-  robot_state_rviz_plugin_description.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-
-install(DIRECTORY icons DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest(test/moveit_joy.test)
-endif()
+# add_subdirectory(motion_planning_rviz_plugin)
+# add_subdirectory(trajectory_rviz_plugin)
+
+install(DIRECTORY icons DESTINATION share)
+
+#pluginlib_export_plugin_description_file(rviz_common motion_planning_rviz_plugin_description.xml)
+#pluginlib_export_plugin_description_file(rviz_common trajectory_rviz_plugin_description.xml)
+pluginlib_export_plugin_description_file(rviz_common planning_scene_rviz_plugin_description.xml)
+pluginlib_export_plugin_description_file(rviz_common robot_state_rviz_plugin_description.xml)
+
+#if (CATKIN_ENABLE_TESTING)
+#  find_package(rostest REQUIRED)
+#  add_rostest(test/moveit_joy.test)
+#endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(class_loader)
+ament_export_dependencies(geometric_shapes)
+ament_export_dependencies(interactive_markers)
+ament_export_dependencies(moveit_ros_planning)
+ament_export_dependencies(moveit_msgs)
+ament_export_dependencies(moveit_core)
+ament_export_dependencies(octomap_msgs)
+ament_export_dependencies(object_recognition_msgs)
+ament_export_dependencies(pluginlib)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(rclpy)
+ament_export_dependencies(rviz2)
+ament_export_dependencies(tf2_eigen)
+ament_export_dependencies(Eigen3)
+ament_export_dependencies(rviz_ogre_vendor)
+ament_package()
diff --git a/moveit_ros/visualization/COLCON_IGNORE b/moveit_ros/visualization/COLCON_IGNORE
diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml
@@ -17,7 +17,7 @@
   <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
   <url type="repository">https://github.com/ros-planning/moveit</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>pkg-config</buildtool_depend>
 
   <build_depend>class_loader</build_depend>
@@ -28,21 +28,22 @@
 
   <depend>geometric_shapes</depend>
   <depend>interactive_markers</depend>
-  <depend>moveit_ros_robot_interaction</depend>
-  <depend>moveit_ros_perception</depend>
+  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_robot_interaction -->
+  <!--depend>moveit_ros_robot_interaction</depend-->
+  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_perception -->
+  <!--depend>moveit_ros_perception</depend-->
   <depend>moveit_ros_planning_interface</depend>
-  <depend>moveit_ros_warehouse</depend>
+  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_warehouse -->
+  <!--depend>moveit_ros_warehouse</depend-->
   <depend>object_recognition_msgs</depend>
   <depend version_gte="1.11.2">pluginlib</depend>
-  <depend>rosconsole</depend>
-  <depend>roscpp</depend>
-  <depend>rospy</depend>
-  <depend>rviz</depend>
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>rviz2</depend>
   <depend>tf2_eigen</depend>
 
-  <test_depend>rostest</test_depend>
-
   <export>
+    <build_type>ament_cmake</build_type>
     <rviz plugin="${prefix}/robot_state_rviz_plugin_description.xml"/>
     <rviz plugin="${prefix}/planning_scene_rviz_plugin_description.xml"/>
     <rviz plugin="${prefix}/motion_planning_rviz_plugin_description.xml"/>

diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt
@@ -1,18 +1,35 @@
 set(MOVEIT_LIB_NAME moveit_planning_scene_rviz_plugin)
 
-add_library(${MOVEIT_LIB_NAME}_core
+add_library(${MOVEIT_LIB_NAME}_core SHARED
   src/planning_scene_display.cpp
-  include/moveit/planning_scene_rviz_plugin/planning_scene_display.h)
+)
 set_target_properties(${MOVEIT_LIB_NAME}_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools ${catkin_LIBRARIES} ${OGRE_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME}_core moveit_rviz_plugin_render_tools rviz_ogre_vendor::OgreMain)
+ament_target_dependencies(${MOVEIT_LIB_NAME}_core
+    rclcpp
+    rviz2
+    moveit_ros_planning_interface
+    moveit_ros_planning
+    moveit_msgs
+    pluginlib
+    Boost
+)
 
-add_library(${MOVEIT_LIB_NAME} src/plugin_init.cpp)
+add_library(${MOVEIT_LIB_NAME} SHARED src/plugin_init.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME} ${MOVEIT_LIB_NAME}_core  Qt5::Widgets)
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+    pluginlib
+)
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION include)
 
 install(TARGETS ${MOVEIT_LIB_NAME}_core ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+ament_export_libraries(
+    ${MOVEIT_LIB_NAME}_core
+    ${MOVEIT_LIB_NAME}
+)