[Servo] Improve support for switching planning group

moveit_ros/moveit_servo/demos/cpp_interface/demo_joint_jog.cpp

@@ -93,6 +93,7 @@ int main(int argc, char* argv[])
   RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage());
   while (rclcpp::ok())
   {
+    servo.updateParams();  // This can be skipped if your application will not be updating parameters.
     const KinematicState joint_state = servo.getNextJointState(joint_jog);
     const StatusCode status = servo.getStatus();
 

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -100,6 +100,7 @@ int main(int argc, char* argv[])
     rclcpp::WallRate tracking_rate(1 / servo_params.publish_period);
     while (!stop_tracking && rclcpp::ok())
     {
+      servo.updateParams();  // This can be skipped if your application will not be updating parameters.
       {
         std::lock_guard<std::mutex> pguard(pose_guard);
         joint_state = servo.getNextJointState(target_pose);

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -97,6 +97,7 @@ int main(int argc, char* argv[])
   RCLCPP_INFO_STREAM(LOGGER, servo.getStatusMessage());
   while (rclcpp::ok())
   {
+    servo.updateParams();  // This can be skipped if your application will not be updating parameters.
     const KinematicState joint_state = servo.getNextJointState(target_twist);
     const StatusCode status = servo.getStatus();
 

moveit_ros/moveit_servo/include/moveit_servo/servo.hpp

@@ -96,6 +96,12 @@ class Servo
    */
   StatusCode getStatus() const;
 
+  /**
+   * \brief Set the current status of servo to the provided status.
+   * @param status The new status of servo.
+   */
+  void setStatus(const StatusCode status);
+
   /**
    * \brief Get the message associated with the current servo status.
    * @return The status message.
@@ -122,14 +128,19 @@ class Servo
    * \brief Get the current state of the robot as given by planning scene monitor.
    * @return The current state of the robot.
    */
-  KinematicState getCurrentRobotState() const;
+  KinematicState getCurrentRobotState();
 
   /**
    * \brief Smoothly halt at a commanded state when command goes stale.
    * @param The last commanded joint states.
    * @return The next state stepping towards the required halting state.
    */
-  std::pair<bool, KinematicState> smoothHalt(const KinematicState& halt_state) const;
+  std::pair<bool, KinematicState> smoothHalt(const KinematicState& halt_state);
+
+  /**
+   * \brief Updates the servo parameters and performs validations.
+   */
+  bool updateParams();
 
 private:
   /**
@@ -139,7 +150,7 @@ class Servo
    * @param command_frame The command frame name.
    * @return The transformation between planning frame and command frame.
    */
-  Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame) const;
+  Eigen::Isometry3d getPlanningToCommandFrameTransform(const std::string& command_frame);
 
   /**
    * \brief Convert a given twist command to planning frame,
@@ -150,21 +161,21 @@ class Servo
    * @param command The twist command to be converted.
    * @return The transformed twist command.
    */
-  TwistCommand toPlanningFrame(const TwistCommand& command) const;
+  TwistCommand toPlanningFrame(const TwistCommand& command);
 
   /**
    * \brief Convert a given pose command to planning frame
    * @param command The pose command to be converted
    * @return The transformed pose command
    */
-  PoseCommand toPlanningFrame(const PoseCommand& command) const;
+  PoseCommand toPlanningFrame(const PoseCommand& command);
 
   /**
    * \brief Compute the change in joint position required to follow the received command.
    * @param command The incoming servo command.
    * @return The joint position change required (delta).
    */
-  Eigen::VectorXd jointDeltaFromCommand(const ServoInput& command, const moveit::core::RobotStatePtr& robot_state);
+  Eigen::VectorXd jointDeltaFromCommand(const ServoInput& command);
 
   /**
    * \brief Updates data depending on joint model group
@@ -178,11 +189,6 @@ class Servo
    */
   bool validateParams(const servo::Params& servo_params) const;
 
-  /**
-   * \brief Updates the servo parameters and performs validations.
-   */
-  bool updateParams();
-
   /**
    * \brief Create and initialize the smoothing plugin to be used by servo.
    */
@@ -214,6 +220,12 @@ class Servo
   std::unique_ptr<CollisionMonitor> collision_monitor_;
 
   pluginlib::UniquePtr<online_signal_smoothing::SmoothingBaseClass> smoother_ = nullptr;
+
+  moveit::core::RobotStatePtr robot_state_;
+  const moveit::core::JointModelGroup* joint_model_group_;
+  std::vector<std::string> joint_names_;
+
+  bool invalid_parameter_update_;
 };
 
 }  // namespace moveit_servo

moveit_ros/moveit_servo/include/moveit_servo/utils/datatypes.hpp

@@ -57,7 +57,8 @@ enum class StatusCode : int8_t
   DECELERATE_FOR_LEAVING_SINGULARITY = 3,
   DECELERATE_FOR_COLLISION = 4,
   HALT_FOR_COLLISION = 5,
-  JOINT_BOUND = 6
+  JOINT_BOUND = 6,
+  PAUSED = 7
 };
 
 const std::unordered_map<StatusCode, std::string> SERVO_STATUS_CODE_MAP(
@@ -68,7 +69,8 @@ const std::unordered_map<StatusCode, std::string> SERVO_STATUS_CODE_MAP(
       { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" },
       { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" },
       { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" },
-      { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } });
+      { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" },
+      { StatusCode::PAUSED, "Servo paused" } });
 
 // The datatype that specifies the type of command that servo should expect.
 enum class CommandType : int8_t