Cleanup + add description

moveit_ros/hybrid_planning/CMakeLists.txt

@@ -38,6 +38,8 @@ set(LIBRARIES
   # Parameters
   local_planner_parameters
   hp_manager_parameters
+  forward_trajectory_parameters
+  servo_solver_parameters
 )
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt

@@ -1,9 +1,13 @@
 # Forward Trajectory Solver
+generate_parameter_library(forward_trajectory_parameters res/forward_trajectory_parameters.yaml)
 add_library(forward_trajectory_plugin SHARED src/forward_trajectory.cpp)
 set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(forward_trajectory_plugin forward_trajectory_parameters)
 ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 # Servo Local Solver
+generate_parameter_library(servo_solver_parameters res/servo_solver_parameters.yaml)
 add_library(servo_local_solver_plugin SHARED src/servo_solver.cpp)
 set_target_properties(servo_local_solver_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+target_link_libraries(servo_local_solver_plugin servo_solver_parameters)
 ament_target_dependencies(servo_local_solver_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS})

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/servo_solver.hpp

@@ -33,8 +33,7 @@
  *********************************************************************/
 
 /* Author: Sebastian Jahr
-   Description: Local solver plugin that uses moveit_servo to execute the local trajectory in combination with
-   replanning capabilities
+   Description: Local solver plugin that uses moveit_servo steer the robot towards a Cartesian goal point.
  */
 
 #pragma once
@@ -44,17 +43,45 @@
 #include <moveit/local_planner/local_constraint_solver_interface.h>
 
 #include <moveit_servo/servo.hpp>
+#include <servo_solver_parameters.hpp>
 
 namespace moveit::hybrid_planning
 {
+/**
+ * @brief Local solver plugin that utilizes moveit_servo as a local planner
+ *
+ */
 class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterface
 {
 public:
+  /**
+   * @brief Initializes moveit servo
+   *
+   * @param node
+   * @param planning_scene_monitor Planning scene monitor to access the current state of the system
+   * @param group_name UNUSED
+   * @return True if initialization was successful
+   */
   bool initialize(const rclcpp::Node::SharedPtr& node,
                   const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                   const std::string& group_name) override;
+  /**
+   * @brief Reset rolling window
+   *
+   * @return Always returns true
+   */
   bool reset() override;
 
+  /**
+   * @brief Solves local planning problem with servo. The first waypoint of the local trajectory is used as goal point
+   * for servo. After computing FK for that waypoint, the difference between the current and goal reference frame is used
+   * to calculate a twist command for servo. The status of servo is returned as feedback to the hybrid planning manager.
+   *
+   * @param local_trajectory Reference trajectory, only the first waypoint is used
+   * @param local_goal UNUSED
+   * @param local_solution Joint trajectory containing a sequence of servo commands
+   * @return moveit_msgs::action::LocalPlanner::Feedback containing the servo status code
+   */
   moveit_msgs::action::LocalPlanner::Feedback
   solve(const robot_trajectory::RobotTrajectory& local_trajectory,
         const std::shared_ptr<const moveit_msgs::action::LocalPlanner::Goal> local_goal,
@@ -63,29 +90,12 @@ class ServoSolver : public moveit::hybrid_planning::LocalConstraintSolverInterfa
 private:
   rclcpp::Node::SharedPtr node_;
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
-
+  servo_solver_parameters::Params solver_parameters_;
   servo::Params servo_parameters_;
 
   // Servo cpp interface
   std::unique_ptr<moveit_servo::Servo> servo_;
 
-  // Uncomment for debugging
-  // rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
-
-  rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr traj_cmd_pub_;
-  bool publish_ = true;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr collision_velocity_scale_sub_;
-
-  // Subscribe to laser corrections
-  rclcpp::Subscription<control_msgs::msg::JointJog>::SharedPtr laser_corrections_sub_;
-  double laser_correction_ = 0;
-
-  // Flag to indicate if replanning is necessary
-  bool replan_;
-
-  // Flag to indicate that replanning is requested
-  bool feedback_send_;
-
   // Command queue to build trajectory message and add current robot state
   std::deque<moveit_servo::KinematicState> joint_cmd_rolling_window_;
 };

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml

@@ -0,0 +1,6 @@
+forward_trajectory_parameters:
+  stop_before_collision: {
+    type: bool,
+    description: "If a collision is detected should the robot stop at the current pose or continue executing",
+    default_value: false,
+  }

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml

@@ -0,0 +1,8 @@
+servo_solver_parameters:
+  reference_frame: {
+    type: string,
+    description: "Frame that is tracked by servo",
+    #validation: {
+    #  not_empty<>: []
+    #}
+  }

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp

@@ -36,6 +36,7 @@
 #include <moveit/local_planner/feedback_types.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/robot_state/conversions.h>
+#include <forward_trajectory_parameters.hpp>
 
 namespace
 {
@@ -50,15 +51,8 @@ bool ForwardTrajectory::initialize(const rclcpp::Node::SharedPtr& node,
                                    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                                    const std::string& /* unused */)
 {
-  // Load parameter & initialize member variables
-  if (node->has_parameter("stop_before_collision"))
-  {
-    node->get_parameter<bool>("stop_before_collision", stop_before_collision_);
-  }
-  else
-  {
-    stop_before_collision_ = node->declare_parameter<bool>("stop_before_collision", false);
-  }
+  auto param_listener = forward_trajectory_parameters::ParamListener(node, "");
+  stop_before_collision_ = param_listener.get_params().stop_before_collision;
   node_ = node;
   path_invalidation_event_send_ = false;
   num_iterations_stuck_ = 0;

moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp

@@ -59,6 +59,10 @@ bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node,
   planning_scene_monitor_ = planning_scene_monitor;
   node_ = node;
 
+  const std::shared_ptr<const servo_solver_parameters::ParamListener> solver_param_listener =
+      std::make_shared<const servo_solver_parameters::ParamListener>(node, "");
+  solver_parameters_ = solver_param_listener->get_params();
+
   // Get Servo Parameters
   // Get the servo parameters.
   const std::string param_namespace = "moveit_servo";
@@ -109,9 +113,9 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
   target_state.update();
 
   // TF planning_frame -> current EE
-  Eigen::Isometry3d current_pose = current_state->getFrameTransform("tcp_welding_gun_link");
+  Eigen::Isometry3d current_pose = current_state->getFrameTransform(solver_parameters_.reference_frame);
   // TF planning -> target EE
-  Eigen::Isometry3d target_pose = target_state.getFrameTransform("tcp_welding_gun_link");
+  Eigen::Isometry3d target_pose = target_state.getFrameTransform(solver_parameters_.reference_frame);
 
   // current EE -> planning frame * planning frame -> target EE
   Eigen::Isometry3d diff_pose = current_pose.inverse() * target_pose;
@@ -127,33 +131,27 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
   // Transform goal pose to command frame
   servo_->setCommandType(moveit_servo::CommandType::TWIST);
   moveit_servo::TwistCommand target_twist{
-    "tcp_welding_gun_link",
+    solver_parameters_.reference_frame,
     { diff_pose.translation().x() * trans_gain, diff_pose.translation().y() * trans_gain,
       diff_pose.translation().z() * trans_gain, axis_angle.axis().x() * axis_angle.angle() * rot_gain,
       axis_angle.axis().y() * axis_angle.angle() * rot_gain, axis_angle.axis().z() * axis_angle.angle() * rot_gain }
   };
 
-  // Start DEBUG uncomment for debugging
-  // auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
-  // msg->header.stamp = node_->now();
-  // msg->twist.linear.x = target_twist.velocities[0];
-  // msg->twist.linear.y = target_twist.velocities[1];
-  // msg->twist.linear.z = target_twist.velocities[2];
-  // msg->twist.angular.x =target_twist.velocities[3];
-  // msg->twist.angular.y =target_twist.velocities[4];
-  // msg->twist.angular.z =target_twist.velocities[5];
-  // twist_cmd_pub_->publish(std::move(msg));
-  // End Debug
-
   std::optional<trajectory_msgs::msg::JointTrajectory> trajectory_msg;
+  // Create servo commands until a trajectory message can be generated
   while (!trajectory_msg)
   {
     // Calculate next servo command
     moveit_servo::KinematicState joint_state = servo_->getNextJointState(current_state, target_twist);
     const auto status = servo_->getStatus();
+    // Servo solver feedback is always the status of the first servo iteration
+    if (feedback_result.feedback.empty())
+    {
+      feedback_result.feedback = moveit_servo::SERVO_STATUS_CODE_MAP.at(status);
+    }
+    // If servo couldn't compute the next joint state, exit local solver without a solution
     if (status == moveit_servo::StatusCode::INVALID)
     {
-      feedback_result.feedback = "Servo StatusCode 'INVALID'";
       return feedback_result;
     }
     moveit_servo::updateSlidingWindow(joint_state, joint_cmd_rolling_window_, servo_parameters_.max_expected_latency,

moveit_ros/hybrid_planning/plugins.xml

@@ -43,4 +43,4 @@
     <description>Uses Servo as a local solver.</description>
   </class>
 </library>
-</class_libraries>
+</class_libraries>