From 674e10f80af1742742fc2ecda2a56ae7f684eb11 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Thu, 25 Jan 2024 12:30:24 +0100 Subject: [PATCH] Small update --- .../res/forward_trajectory_parameters.yaml | 2 +- .../res/servo_solver_parameters.yaml | 8 ++++---- .../local_constraint_solver_plugins/src/servo_solver.cpp | 3 +-- 3 files changed, 6 insertions(+), 7 deletions(-) diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml index 588c003b14e..181b982f33a 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/forward_trajectory_parameters.yaml +++ b/moveit_ros/hybrid_planning/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, - } \ No newline at end of file + } diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml index 26fa05ad14f..67646014d75 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/res/servo_solver_parameters.yaml +++ b/moveit_ros/hybrid_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<>: [] - #} - } \ No newline at end of file + validation: { + not_empty<>: [] + } + } diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp index 8f96f9b37cc..a351ee20fdf 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/servo_solver.cpp +++ b/moveit_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 servo_param_listener = std::make_shared(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); }