diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h index ab2c3064b09..13cf36a742c 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.h @@ -94,13 +94,11 @@ class RuckigFilterPlugin : public SmoothingBaseClass bool getVelAccelJerkBounds(std::vector& joint_velocity_bounds, std::vector& joint_acceleration_bounds, std::vector& joint_jerk_bounds); - rclcpp::Node::SharedPtr node_; /** \brief Parameters loaded from yaml file at runtime */ online_signal_smoothing::Params params_; - size_t num_joints_; /** \brief The robot model contains the vel/accel/jerk limits that Ruckig requires */ moveit::core::RobotModelConstPtr robot_model_; - bool have_initial_ruckig_output_; + bool have_initial_ruckig_output_ = false; std::optional> ruckig_; std::optional> ruckig_input_; std::optional> ruckig_output_; diff --git a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp index bddb3fed818..2c896d04e79 100644 --- a/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp +++ b/moveit_core/online_signal_smoothing/src/ruckig_filter.cpp @@ -52,31 +52,28 @@ rclcpp::Logger getLogger() bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, size_t num_joints) { - node_ = node; - num_joints_ = num_joints; robot_model_ = robot_model; - have_initial_ruckig_output_ = false; // get node parameters and store in member variables - auto param_listener = online_signal_smoothing::ParamListener(node_); + auto param_listener = online_signal_smoothing::ParamListener(node); params_ = param_listener.get_params(); // Ruckig needs the joint vel/accel bounds // TODO: Ruckig says the jerk bounds can be optional. We require them, for now. - ruckig::InputParameter ruckig_input(num_joints_); + ruckig::InputParameter ruckig_input(num_joints); if (!getVelAccelJerkBounds(ruckig_input.max_velocity, ruckig_input.max_acceleration, ruckig_input.max_jerk)) { return false; } - ruckig_input.current_position = std::vector(num_joints_, 0.0); - ruckig_input.current_velocity = std::vector(num_joints_, 0.0); - ruckig_input.current_acceleration = std::vector(num_joints_, 0.0); + ruckig_input.current_position = std::vector(num_joints, 0.0); + ruckig_input.current_velocity = std::vector(num_joints, 0.0); + ruckig_input.current_acceleration = std::vector(num_joints, 0.0); ruckig_input_ = ruckig_input; - ruckig_output_.emplace(ruckig::OutputParameter(num_joints_)); + ruckig_output_.emplace(ruckig::OutputParameter(num_joints)); - ruckig_.emplace(ruckig::Ruckig(num_joints_, params_.update_period)); + ruckig_.emplace(ruckig::Ruckig(num_joints, params_.update_period)); return true; } @@ -84,6 +81,12 @@ bool RuckigFilterPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core:: bool RuckigFilterPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) { + if (!ruckig_input_ || !ruckig_output_ || !ruckig_) + { + RCLCPP_ERROR_STREAM(getLogger(), "The Ruckig smoother was not initialized"); + return false; + } + if (have_initial_ruckig_output_) { ruckig_output_->pass_to_input(*ruckig_input_); @@ -141,6 +144,12 @@ bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_veloci std::vector& joint_acceleration_bounds, std::vector& joint_jerk_bounds) { + if (!robot_model_) + { + RCLCPP_ERROR(getLogger(), "The robot model was not initialized."); + return false; + } + joint_velocity_bounds.clear(); joint_acceleration_bounds.clear(); joint_jerk_bounds.clear(); @@ -157,8 +166,8 @@ bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_veloci } else { - RCLCPP_ERROR(getLogger(), "No joint velocity limit defined. Exiting for safety."); - std::exit(EXIT_FAILURE); + RCLCPP_ERROR(getLogger(), "No joint velocity limit defined for " << joint->getName() << "."); + return false; } if (bound.acceleration_bounded_) @@ -168,9 +177,8 @@ bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_veloci } else { - RCLCPP_WARN(getLogger(), "No joint acceleration limit defined. Very large accelerations will be " - "possible."); - joint_acceleration_bounds.push_back(DBL_MAX); + RCLCPP_WARN_STREAM(getLogger(), "No joint acceleration limit defined for " << joint->getName() << "."); + return false; } if (bound.jerk_bounded_) @@ -181,7 +189,8 @@ bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_veloci // else, return false else { - RCLCPP_WARN(getLogger(), "No joint jerk limit was defined. The output from Ruckig will not be jerk-limited."); + RCLCPP_WARN_STREAM(getLogger(), "No joint jerk limit was defined for " + << joint->getName() << ". The output from Ruckig will not be jerk-limited."); return false; } }