Small update

sjahr · Jan 25, 2024 · 674e10f · 674e10f
1 parent 4cce9e2
commit 674e10f
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 7 deletions.
diff --git a/...ning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml b/...ning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml
@@ -3,4 +3,4 @@ forward_trajectory_parameters:
     type: bool,
     description: "If a collision is detected should the robot stop at the current pose or continue executing",
     default_value: false,
-  }
+  }
diff --git a/...d_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml b/...d_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml
@@ -2,7 +2,7 @@ servo_solver_parameters:
   reference_frame: {
     type: string,
     description: "Frame that is tracked by servo",
-    #validation: {
-    #  not_empty<>: []
-    #}
-  }
+    validation: {
+      not_empty<>: []
+    }
+  }
diff --git a/...it_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/...it_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp
@@ -64,7 +64,6 @@ bool ServoSolver::initialize(const rclcpp::Node::SharedPtr& node,
   solver_parameters_ = solver_param_listener->get_params();
 
   // Get Servo Parameters
-  // Get the servo parameters.
   const std::string param_namespace = "moveit_servo";
   const std::shared_ptr<const servo::ParamListener> servo_param_listener =
       std::make_shared<const servo::ParamListener>(node, param_namespace);
@@ -145,7 +144,7 @@ ServoSolver::solve(const robot_trajectory::RobotTrajectory& local_trajectory,
     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())
+    if (feedback_result.feedback.empty() && status != moveit_servo::StatusCode::NO_WARNING)
     {
       feedback_result.feedback = moveit_servo::SERVO_STATUS_CODE_MAP.at(status);
     }