[Servo] Update MoveIt Servo to use generate_parameter_library (moveit…

…#2096) * Add generate_parameter_library as dependency * Add parameters file * Update parameters file * Fix one_of syntax * Add parameter generation * Include servo param header * Test if parameters are loaded * Make servo_node partially use ParamListener * Make Servo partially use ParamListener * Make ServoCalcs partially use ParamListener * Fix frame name * Handle parameter updates * Remove old param lib dependency in CollisionCheck * Remove old param lib dependency in ServoCalcs * Remove old param lib dependency in Servo * Remove old param lib dependency in ServoNode * Remove old parameter librarysources * Remove parameter_descriptor_builder sources * Update parameter library header name * Formatting * Remove old param lib headers * Add parameter to enable/disable continous parameter update check * Update pose tracking demo * Fix launch time parameter loading for pose tracking * Move PID parameters to generate_parameter_library * Fix launch time parameter loading for servo example * Fix unit tests * Fix interface test * Fix pose tracking test * Redorder member variable initialization * Cleanup * Group parameters * Make parameter listener const * Revert disabled lint tests * Fix issues from rebase * Apply performance suggestion from CI * Apply variable naming suggestion from CI * Apply pass params by reference suggestion by CI * Apply review suggestions * Apply review suggestions * Remove unused parameter * Change parameter listener to unique_ptr * Add validations for some parameters * Changes from review * Make docstring more informative Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> * Change validation failure from warning to error * Fix parameter loading in test launch files * Remove defaults for robot specific params * Update description for params with no default value * Pass by reference * Clang-tidy Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --------- Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Co-authored-by: AndyZe <andyz@utexas.edu>
PickNikRobotics · Apr 27, 2023 · dc76435 · dc76435
1 parent f432978
commit dc76435
Show file tree

Hide file tree

Showing 27 changed files with 681 additions and 1,230 deletions.
diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt
@@ -26,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
 find_package(ament_cmake REQUIRED)
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
   find_package(${Dependency} REQUIRED)
 endforeach()
@@ -39,12 +40,10 @@ include_directories(
 ###################
 
 # This library provides a way of loading parameters for servo
-add_library(moveit_servo_lib_parameters SHARED
-  src/servo_parameters.cpp
-  src/parameter_descriptor_builder.cpp
+generate_parameter_library(
+  moveit_servo_lib_parameters
+  src/servo_parameters.yaml
 )
-set_target_properties(moveit_servo_lib_parameters PROPERTIES VERSION "${moveit_servo_VERSION}")
-ament_target_dependencies(moveit_servo_lib_parameters ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # This library provides a C++ interface for sending realtime twist or joint commands to a robot
 add_library(moveit_servo_lib SHARED

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h
@@ -46,8 +46,8 @@
 #include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <std_msgs/msg/float64.hpp>
-
-#include <moveit_servo/servo_parameters.h>
+// Auto-generated
+#include <moveit_servo_lib_parameters.hpp>
 
 namespace moveit_servo
 {
@@ -59,7 +59,7 @@ class CollisionCheck
    *  \param planning_scene_monitor: PSM should have scene monitor and state monitor
    *                                 already started when passed into this class
    */
-  CollisionCheck(const rclcpp::Node::SharedPtr& node, const ServoParameters::SharedConstPtr& parameters,
+  CollisionCheck(const rclcpp::Node::SharedPtr& node, const servo::Params& servo_params,
                  const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
 
   ~CollisionCheck()
@@ -86,8 +86,7 @@ class CollisionCheck
   // Pointer to the ROS node
   const std::shared_ptr<rclcpp::Node> node_;
 
-  // Parameters from yaml
-  const ServoParameters::SharedConstPtr parameters_;
+  servo::Params servo_params_;
 
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp b/moveit_ros/moveit_servo/include/moveit_servo/parameter_descriptor_builder.hpp
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h b/moveit_ros/moveit_servo/include/moveit_servo/pose_tracking.h
@@ -41,8 +41,8 @@
 #include <atomic>
 #include <control_toolbox/pid.hpp>
 #include <moveit_servo/make_shared_from_pool.h>
-#include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/servo.h>
+#include <moveit_servo_lib_parameters.hpp>
 #include <optional>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #include <tf2_ros/transform_listener.h>
@@ -87,7 +87,7 @@ class PoseTracking
 {
 public:
   /** \brief Constructor. Loads ROS parameters under the given namespace. */
-  PoseTracking(const rclcpp::Node::SharedPtr& node, const ServoParameters::SharedConstPtr& servo_parameters,
+  PoseTracking(const rclcpp::Node::SharedPtr& node, std::unique_ptr<const servo::ParamListener> servo_param_listener,
                const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
 
   PoseTrackingStatusCode moveToPose(const Eigen::Vector3d& positional_tolerance, const double angular_tolerance,
@@ -152,7 +152,7 @@ class PoseTracking
   void doPostMotionReset();
 
   rclcpp::Node::SharedPtr node_;
-  moveit_servo::ServoParameters::SharedConstPtr servo_parameters_;
+  servo::Params servo_parameters_;
 
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
   moveit::core::RobotModelConstPtr robot_model_;

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h
@@ -43,8 +43,8 @@
 
 // Moveit2
 #include <moveit_servo/collision_check.h>
-#include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/servo_calcs.h>
+#include <moveit_servo_lib_parameters.hpp>
 
 namespace moveit_servo
 {
@@ -54,8 +54,9 @@ namespace moveit_servo
 class Servo
 {
 public:
-  Servo(const rclcpp::Node::SharedPtr& node, const ServoParameters::SharedConstPtr& parameters,
-        const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
+  Servo(const rclcpp::Node::SharedPtr& node,
+        const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+        std::unique_ptr<const servo::ParamListener> servo_param_listener);
 
   ~Servo();
 
@@ -85,19 +86,14 @@ class Servo
   bool getEEFrameTransform(Eigen::Isometry3d& transform);
   bool getEEFrameTransform(geometry_msgs::msg::TransformStamped& transform);
 
-  /** \brief Get the parameters used by servo node. */
-  const ServoParameters::SharedConstPtr& getParameters() const;
-
   // Give test access to private/protected methods
   friend class ServoFixture;
 
 private:
+  servo::Params servo_params_;
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 
-  // The stored servo parameters
-  ServoParameters::SharedConstPtr parameters_;
-
   ServoCalcs servo_calcs_;
   CollisionCheck collision_checker_;
 };

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
@@ -64,9 +64,9 @@
 #include <moveit/kinematics_base/kinematics_base.h>
 
 // moveit_servo
-#include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/status_codes.h>
 #include <moveit/online_signal_smoothing/smoothing_base_class.h>
+#include <moveit_servo_lib_parameters.hpp>
 
 namespace moveit_servo
 {
@@ -80,8 +80,8 @@ class ServoCalcs
 {
 public:
   ServoCalcs(const rclcpp::Node::SharedPtr& node,
-             const std::shared_ptr<const moveit_servo::ServoParameters>& parameters,
-             const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
+             const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
+             std::unique_ptr<const servo::ParamListener> servo_param_listener);
 
   ~ServoCalcs();
 
@@ -92,6 +92,12 @@ class ServoCalcs
    */
   void start();
 
+  /**
+   * Check for parameter update, and apply updates if any
+   * All dynamic parameters must be checked and updated within this method
+   */
+  void updateParams();
+
   /**
    * Get the MoveIt planning link transform.
    * The transform from the MoveIt planning frame to robot_link_command_frame
@@ -246,8 +252,9 @@ class ServoCalcs
   // Pointer to the ROS node
   std::shared_ptr<rclcpp::Node> node_;
 
-  // Parameters from yaml
-  const std::shared_ptr<const moveit_servo::ServoParameters> parameters_;
+  // Servo parameters
+  std::unique_ptr<const servo::ParamListener> servo_param_listener_;
+  servo::Params servo_params_;
 
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
@@ -327,12 +334,6 @@ class ServoCalcs
   std::condition_variable input_cv_;
   bool new_input_cmd_ = false;
 
-  // dynamic parameters
-  std::string robot_link_command_frame_;
-  rcl_interfaces::msg::SetParametersResult robotLinkCommandFrameCallback(const rclcpp::Parameter& parameter);
-  double override_velocity_scaling_factor_;
-  rcl_interfaces::msg::SetParametersResult overrideVelocityScalingFactorCallback(const rclcpp::Parameter& parameter);
-
   // Load a smoothing plugin
   pluginlib::ClassLoader<online_signal_smoothing::SmoothingBaseClass> smoothing_loader_;
 

diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_node.h
@@ -42,6 +42,7 @@
 
 #include <moveit_servo/servo.h>
 #include <std_srvs/srv/trigger.hpp>
+#include <moveit_servo_lib_parameters.hpp>
 
 namespace moveit_servo
 {
@@ -62,6 +63,13 @@ class ServoNode
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
 
+  /***
+   * \brief Most servo parameters are individually validated using the validation methods in
+   * `generate_parameter_library`, with limits specified in the parameters YAML file. This method performs additional
+   * validation for parameters whose values depend on each other.
+   */
+  void validateParams(const servo::Params& servo_params);
+
   /** \brief Start the servo loop. Must be called once to begin Servoing. */
   void startCB(const std::shared_ptr<std_srvs::srv::Trigger::Request>& request,
                const std::shared_ptr<std_srvs::srv::Trigger::Response>& response);