diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index eba0b54073..3306fd0063 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -32,7 +32,7 @@ jobs: ROS_DISTRO: jazzy env: CXXFLAGS: >- - -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option + -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }} UPSTREAM_WORKSPACE: > 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 ab2c3064b0..13cf36a742 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 bddb3fed81..338a5badd7 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(); @@ -158,7 +167,7 @@ bool RuckigFilterPlugin::getVelAccelJerkBounds(std::vector& joint_veloci else { RCLCPP_ERROR(getLogger(), "No joint velocity limit defined. Exiting for safety."); - std::exit(EXIT_FAILURE); + return false; } if (bound.acceleration_bounded_)