From ae90d19723eeec23d5050c9baaffcc6e2e359162 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 13 May 2019 19:20:42 +0200 Subject: [PATCH 1/5] Adding node to planning_interface --- .../include/moveit/planning_interface/planning_interface.h | 2 +- moveit_core/planning_interface/src/planning_interface.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 dd9c16241e..46f4b801e2 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 @@ -165,7 +165,7 @@ class PlannerManager /// It is assumed that motion plans will be computed for the robot described by \e model and that any exposed ROS /// functionality /// or required ROS parameters are namespaced by \e ns - virtual bool initialize(const robot_model::RobotModelConstPtr& model, const std::string& ns); + virtual bool initialize(const robot_model::RobotModelConstPtr& model, std::shared_ptr& node); /// Get \brief a short string that identifies the planning interface virtual std::string getDescription() const; diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index e093130090..b8b4610cd8 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -93,7 +93,7 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); } -bool PlannerManager::initialize(const robot_model::RobotModelConstPtr& /*unused*/, const std::string& /*unused*/) +bool PlannerManager::initialize(const robot_model::RobotModelConstPtr& /*unused*/, std::shared_ptr& node) { return true; } From 5159b383535df4cd5a34a3efea58d43d7219b1b0 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 15:29:12 +0200 Subject: [PATCH 2/5] moveit_core planning_interface - library as shared --- moveit_core/planning_interface/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 58ba40bc87..b7571d8f1b 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -1,6 +1,6 @@ set(MOVEIT_LIB_NAME moveit_planning_interface) -add_library(${MOVEIT_LIB_NAME} +add_library(${MOVEIT_LIB_NAME} SHARED src/planning_interface.cpp src/planning_response.cpp ) From 1361cfd5866962a889ea98321cbb0045701c1320 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 15:30:06 +0200 Subject: [PATCH 3/5] moveit_core planning_interface - fixing CMakeLists.txt --- moveit_core/planning_interface/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index b7571d8f1b..e689260011 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -13,6 +13,10 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdfdom urdfdom_headers) +target_link_libraries(${MOVEIT_LIB_NAME} + moveit_robot_trajectory + moveit_robot_state) + install(TARGETS ${MOVEIT_LIB_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib) From 1aaa02ed1374f2e9bd64bc35350e04c912efdbc5 Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Wed, 29 May 2019 12:47:45 -0600 Subject: [PATCH 4/5] clang --- moveit_core/planning_interface/src/planning_interface.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index b8b4610cd8..b778e62fa0 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -82,14 +82,13 @@ void PlanningContext::setMotionPlanRequest(const MotionPlanRequest& request) request_ = request; if (request_.allowed_planning_time <= 0.0) { - RCLCPP_INFO(LOGGER, - "The timeout for planning must be positive (%lf specified). Assuming one second instead.", - request_.allowed_planning_time); + 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. " - "Assuming one attempt."); + "Assuming one attempt."); request_.num_planning_attempts = std::max(1, request_.num_planning_attempts); } From 99bc9d2d64abfeb358ad060c3d8cf7d673be5f2a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 29 May 2019 23:29:20 +0200 Subject: [PATCH 5/5] moveit_core planning_interface - update docstrings --- .../include/moveit/planning_interface/planning_interface.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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 46f4b801e2..d1e1630c65 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 @@ -162,9 +162,8 @@ class PlannerManager /// Initialize a planner. This function will be called after the construction of the plugin, before any other call is /// made. - /// It is assumed that motion plans will be computed for the robot described by \e model and that any exposed ROS - /// functionality - /// or required ROS parameters are namespaced by \e ns + /// It is assumed that motion plans will be computed for the robot described by \e model + /// and the node is passed as an argument to get some ROS parameters virtual bool initialize(const robot_model::RobotModelConstPtr& model, std::shared_ptr& node); /// Get \brief a short string that identifies the planning interface