Cleanup ControllerManagers

moveit_core/CMakeLists.txt

@@ -189,6 +189,7 @@ add_subdirectory(macros)
 add_subdirectory(backtrace)
 add_subdirectory(exceptions)
 add_subdirectory(profiler)
+add_subdirectory(utils)
 add_subdirectory(background_processing)
 add_subdirectory(kinematics_base)
 add_subdirectory(controller_manager)
@@ -209,4 +210,3 @@ add_subdirectory(distance_field)
 add_subdirectory(collision_distance_field)
 add_subdirectory(kinematics_metrics)
 add_subdirectory(dynamics_solver)
-add_subdirectory(utils)

moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h

@@ -120,18 +120,24 @@ class MoveItControllerHandle
     return name_;
   }
 
-  /** \brief Send a trajectory to the controller. The controller is expected to execute the trajectory, but this
-   * function call should not block. Blocking is achievable by calling waitForExecution(). Return false when the
-   * controller cannot accept the trajectory. */
+  /** \brief Send a trajectory to the controller.
+   *
+   * The controller is expected to execute the trajectory, but this function call should not block.
+   * Blocking is achievable by calling waitForExecution().
+   * Return false when the controller cannot accept the trajectory. */
   virtual bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) = 0;
 
-  /** \brief Cancel the execution of any motion using this controller. Report false if canceling is not possible. If
-   * there is no execution in progress, this function is a no-op and returns true. */
+  /** \brief Cancel the execution of any motion using this controller.
+   *
+   * Report false if canceling is not possible.
+   * If there is no execution in progress, this function is a no-op and returns true. */
   virtual bool cancelExecution() = 0;
 
-  /** \brief Wait for the current execution to complete, or until the timeout is reached. Return true if the execution
-   * is complete (whether successful or not). Return false if timeout was reached. If timeout is 0 (default argument),
-   * wait until the execution is complete (no timeout). */
+  /** \brief Wait for the current execution to complete, or until the timeout is reached.
+   *
+   * Return true if the execution is complete (whether successful or not).
+   * Return false if timeout was reached.
+   * If timeout is 0 (default argument), wait until the execution is complete (no timeout). */
   virtual bool waitForExecution(const ros::Duration& timeout = ros::Duration(0)) = 0;
 
   /** \brief Return the execution status of the last trajectory sent to the controller. */
@@ -143,12 +149,10 @@ class MoveItControllerHandle
 
 MOVEIT_CLASS_FORWARD(MoveItControllerManager);
 
-/** @brief MoveIt! does not enforce how controllers are
-    implemented. To make your controllers usable by MoveIt, this
-    interface needs to be implemented. The main purpose of this
-    interface is to expose the set of known controllers and
-    potentially to allow activating and deactivating them, if multiple
-    controllers are available.
+/** @brief MoveIt! does not enforce how controllers are implemented.
+    To make your controllers usable by MoveIt, this interface needs to be implemented.
+    The main purpose of this interface is to expose the set of known controllers and
+    potentially to allow activating and deactivating them, if multiple controllers are available.
  */
 class MoveItControllerManager
 {
@@ -179,20 +183,22 @@ class MoveItControllerManager
   {
   }
 
-  /** \brief Controllers are managed by name. Given a name, return an object that can perform operations on the
-   * corresponding controller. */
+  /** \brief Return a given named controller. */
   virtual MoveItControllerHandlePtr getControllerHandle(const std::string& name) = 0;
 
   /** \brief Get the list of known controller names. */
   virtual void getControllersList(std::vector<std::string>& names) = 0;
 
-  /** \brief Get the list of active controllers. If there is only one controller in the system, this will be active. In
-   * cases where multiple controllers exist, and they operate on overlaping sets of joints, not all controllers should
-   * be active at the same time. */
+  /** \brief Get the list of active controllers.
+   *
+   * If there is only one controller in the system, this will be active.  When multiple controllers exist,
+   * and they operate on overlaping sets of joints, not all controllers should be active at the same time. */
   virtual void getActiveControllers(std::vector<std::string>& names) = 0;
 
-  /** \brief In order to decide which controller to use, it is necessary to reason about the joints a controller
-   * operates on. This function reports the joints a controller operates on, given the controller name. */
+  /** \brief Report the joints a controller operates on, given the controller name.
+   *
+   * In order to decide which controller to use, it is necessary to reason about the joints a controller
+   * operates on. */
   virtual void getControllerJoints(const std::string& name, std::vector<std::string>& joints) = 0;
 
   /** \brief Report the state of a controller, given its name. */

moveit_core/kinematic_constraints/CMakeLists.txt

@@ -6,7 +6,7 @@ add_library(${MOVEIT_LIB_NAME}
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 target_link_libraries(${MOVEIT_LIB_NAME}
-  moveit_robot_model moveit_kinematics_base moveit_robot_state moveit_collision_detection_fcl
+  moveit_robot_model moveit_kinematics_base moveit_robot_state moveit_collision_detection_fcl moveit_utils
   ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
 add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
 

moveit_core/kinematic_constraints/src/utils.cpp

@@ -37,9 +37,9 @@
 #include <moveit/kinematic_constraints/utils.h>
 #include <geometric_shapes/solid_primitive_dims.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <xmlrpcpp/XmlRpcValue.h>
+#include <moveit/utils/xmlrpc_casts.h>
 
-#include <boost/algorithm/string/join.hpp>
+using namespace moveit::core;
 
 namespace kinematic_constraints
 {
@@ -274,49 +274,6 @@ moveit_msgs::Constraints constructGoalConstraints(const std::string& link_name,
   return goal;
 }
 
-static double parseDouble(XmlRpc::XmlRpcValue& v)
-{
-  if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
-    return static_cast<double>(v);
-  else if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
-    return static_cast<int>(v);
-  else
-    return 0.0;
-}
-
-static bool isArray(XmlRpc::XmlRpcValue& v, size_t size, const std::string& name = "",
-                    const std::string& description = "")
-{
-  if (v.getType() != XmlRpc::XmlRpcValue::TypeArray || static_cast<size_t>(v.size()) != size)
-  {
-    if (!name.empty())
-      ROS_WARN_STREAM_NAMED(LOGNAME, name << " is not an array[" << size << "] of " << description);
-    return false;
-  }
-  return true;
-}
-
-static bool isStruct(XmlRpc::XmlRpcValue& v, const std::set<std::string>& keys, const std::string& name = "")
-{
-  if (v.getType() != XmlRpc::XmlRpcValue::TypeStruct)
-  {
-    if (!name.empty())
-      ROS_WARN_STREAM_NAMED(LOGNAME, name << " is not a struct with keys " << boost::join(keys, ","));
-    return false;
-  }
-
-  for (const std::string& key : keys)
-    if (!v.hasMember(key))
-    {
-      if (!name.empty())
-        ROS_WARN_STREAM_NAMED(LOGNAME, name << " is not a struct with keys " << boost::join(keys, ",") << " (misses "
-                                            << key << ")");
-      return false;
-    }
-
-  return true;
-}
-
 static bool constructPoseStamped(XmlRpc::XmlRpcValue::iterator& it, geometry_msgs::PoseStamped& pose)
 {
   if (!isStruct(it->second, { "frame_id", "position", "orientation" }, it->first))

moveit_core/utils/CMakeLists.txt

@@ -1,7 +1,9 @@
 set(MOVEIT_LIB_NAME moveit_utils)
 
 add_library(${MOVEIT_LIB_NAME}
-  src/lexical_casts.cpp)
+  src/lexical_casts.cpp
+  src/xmlrpc_casts.cpp
+)
 target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

moveit_core/utils/include/moveit/utils/xmlrpc_casts.h

@@ -0,0 +1,64 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Hamburg University
+ *  All rights reserved.
+ *
+ *  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 name of Hamburg University 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.
+ *********************************************************************/
+
+/* Author: Michael Görner, Robert Haschke */
+
+#pragma once
+#include <string>
+#include <xmlrpcpp/XmlRpc.h>
+
+namespace moveit
+{
+namespace core
+{
+/// parse a double value from a scalar XmlRpc
+double parseDouble(XmlRpc::XmlRpcValue& v);
+
+/** check that v is an array of given size
+ *
+ * @param size: check that array has given size, zero value allows for array size
+ * @param name: if non-empty, print a warning message "name is not an array[size]"
+ * @param description: if non-empty, serves as a descriptor for array items
+ */
+bool isArray(XmlRpc::XmlRpcValue& v, size_t size = 0, const std::string& name = "",
+             const std::string& description = "");
+
+/** check that v is a struct with given keys
+ *
+ * @param keys: list of required keys
+ * @param name: if non-empty, print a warning message "name is not a struct with keys ..."
+ */
+bool isStruct(XmlRpc::XmlRpcValue& v, const std::vector<std::string>& keys = {}, const std::string& name = "");
+}
+}

moveit_core/utils/src/xmlrpc_casts.cpp

@@ -0,0 +1,91 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Hamburg University
+ *  All rights reserved.
+ *
+ *  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 name of Hamburg University 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.
+ *********************************************************************/
+
+/* Author: Michael Görner, Robert Haschke */
+
+#include <moveit/utils/xmlrpc_casts.h>
+#include <ros/console.h>
+#include <boost/algorithm/string/join.hpp>
+
+namespace moveit
+{
+namespace core
+{
+double parseDouble(XmlRpc::XmlRpcValue& v)
+{
+  if (v.getType() == XmlRpc::XmlRpcValue::TypeDouble)
+    return static_cast<double>(v);
+  else if (v.getType() == XmlRpc::XmlRpcValue::TypeInt)
+    return static_cast<int>(v);
+  else
+    return 0.0;
+}
+
+bool isArray(XmlRpc::XmlRpcValue& v, size_t size, const std::string& name, const std::string& description)
+{
+  if (v.getType() != XmlRpc::XmlRpcValue::TypeArray || (size != 0 && static_cast<size_t>(v.size()) != size))
+  {
+    if (!name.empty())
+      ROS_WARN_STREAM(name << " is not an array[" << size << "] of "
+                           << (description.empty() ? "elements" : description.c_str()));
+    return false;
+  }
+  return true;
+}
+
+bool isStruct(XmlRpc::XmlRpcValue& v, const std::vector<std::string>& keys, const std::string& name)
+{
+  if (v.getType() != XmlRpc::XmlRpcValue::TypeStruct)
+  {
+    if (!name.empty())
+      ROS_WARN_STREAM(name << " is not a struct with keys " << boost::join(keys, ","));
+    return false;
+  }
+
+  std::vector<std::string> missing;
+  for (const std::string& key : keys)
+    if (!v.hasMember(key))
+      missing.push_back(key);
+
+  if (!name.empty() && !missing.empty())
+  {
+    ROS_WARN_STREAM(name << " is not a struct with keys " << boost::join(keys, ",") << " (misses "
+                         << boost::join(missing, ",") << ")");
+    return false;
+  }
+
+  return missing.empty();
+}
+}
+}

moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt

@@ -25,7 +25,10 @@ catkin_package(
   CATKIN_DEPENDS moveit_core actionlib control_msgs
 )
 
-add_library(${PROJECT_NAME} src/moveit_simple_controller_manager.cpp)
+add_library(${PROJECT_NAME}
+  src/moveit_simple_controller_manager.cpp
+  src/follow_joint_trajectory_controller_handle.cpp
+)
 set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 

moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h

@@ -57,6 +57,9 @@ class ActionBasedControllerHandleBase : public moveit_controller_manager::MoveIt
 
   virtual void addJoint(const std::string& name) = 0;
   virtual void getJoints(std::vector<std::string>& joints) = 0;
+  virtual void configure(XmlRpc::XmlRpcValue& config)
+  {
+  }
 };
 
 MOVEIT_CLASS_FORWARD(ActionBasedControllerHandleBase);