From dd4e24bf2f74dae43759ff337c8e55a5d74a3e37 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 30 Mar 2019 23:38:19 +0100 Subject: [PATCH 1/2] Port planning_interface to ROS 2 --- moveit_core/planning_interface/CMakeLists.txt | 17 ++++++++++------- .../planning_interface/planning_interface.h | 1 + .../planning_interface/planning_request.h | 2 +- .../planning_interface/planning_response.h | 6 +++--- .../src/planning_interface.cpp | 8 +++++--- 5 files changed, 20 insertions(+), 14 deletions(-) diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 33c0e1a789..58ba40bc87 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -5,12 +5,15 @@ add_library(${MOVEIT_LIB_NAME} src/planning_response.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state moveit_robot_trajectory ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + urdf + visualization_msgs + moveit_robot_state + moveit_robot_trajectory + urdfdom + urdfdom_headers) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) - -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h index 5993d45cb7..dd9c16241e 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_interface.h @@ -42,6 +42,7 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace planning_scene { diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h index 42570a9284..c53abe64b2 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_request.h @@ -37,7 +37,7 @@ #ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_REQUEST_ #define MOVEIT_PLANNING_INTERFACE_PLANNING_REQUEST_ -#include +#include namespace planning_interface { diff --git a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h index 82f6b1effd..ff31004842 100644 --- a/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h +++ b/moveit_core/planning_interface/include/moveit/planning_interface/planning_response.h @@ -38,9 +38,9 @@ #define MOVEIT_PLANNING_INTERFACE_PLANNING_RESPONSE_ #include -#include -#include -#include +#include +#include +#include namespace planning_interface { diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 74293845f3..5b247b34e6 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -40,6 +40,8 @@ namespace planning_interface { + rclcpp::Logger logger = rclcpp::get_logger("planning_interface"); + namespace { // keep track of currently active contexts @@ -80,13 +82,13 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_ = request; if (request_.allowed_planning_time <= 0.0) { - ROS_INFO_NAMED("planning_interface", + RCLCPP_INFO(logger, "The timeout for planning must be positive (%lf specified). Assuming one second instead.", request_.allowed_planning_time); request_.allowed_planning_time = 1.0; } if (request_.num_planning_attempts < 0) - ROS_ERROR_NAMED("planning_interface", "The number of desired planning attempts should be positive. " + RCLCPP_ERROR(logger, "The number of desired planning attempts should be positive. " "Assuming one attempt."); request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); } @@ -127,4 +129,4 @@ void PlannerManager::terminate() const (*it)->terminate(); } -} // end of namespace planning_interface \ No newline at end of file +} // end of namespace planning_interface From 3d03f3d5e61056895b26fa3e9359b5da008a1368 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sat, 30 Mar 2019 23:40:37 +0100 Subject: [PATCH 2/2] planning_interface, adapt logger to https://github.com/ros-planning/moveit2/issues/21 --- moveit_core/planning_interface/src/planning_interface.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 5b247b34e6..e093130090 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -40,7 +40,7 @@ namespace planning_interface { - rclcpp::Logger logger = rclcpp::get_logger("planning_interface"); +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("planning_interface"); namespace { @@ -82,13 +82,13 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_ = request; if (request_.allowed_planning_time <= 0.0) { - RCLCPP_INFO(logger, + RCLCPP_INFO(LOGGER, "The timeout for planning must be positive (%lf specified). Assuming one second instead.", request_.allowed_planning_time); request_.allowed_planning_time = 1.0; } if (request_.num_planning_attempts < 0) - RCLCPP_ERROR(logger, "The number of desired planning attempts should be positive. " + RCLCPP_ERROR(LOGGER, "The number of desired planning attempts should be positive. " "Assuming one attempt."); request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); }