From 39874571571854e87e3068085998083339e36f30 Mon Sep 17 00:00:00 2001 From: chameau5050 Date: Sun, 7 Apr 2024 15:36:46 -0400 Subject: [PATCH] fix all uncrustify and cpplint error --- .../gazebo_ros2_control/gazebo_system.hpp | 9 +- gazebo_ros2_control/src/gazebo_system.cpp | 176 ++++++++++-------- ...rtical_cart_example_velocity_pid.launch.py | 1 - 3 files changed, 104 insertions(+), 82 deletions(-) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp index 64b45341..d3de0eef 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp @@ -43,7 +43,7 @@ class GazeboSystemPrivate; class GazeboSystem : public GazeboSystemInterface { - public: +public: // Documentation Inherited CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) override; @@ -82,7 +82,7 @@ class GazeboSystem : public GazeboSystemInterface const hardware_interface::HardwareInfo & hardware_info, sdf::ElementPtr sdf) override; - private: +private: void registerJoints( const hardware_interface::HardwareInfo & hardware_info, gazebo::physics::ModelPtr parent_model); @@ -91,8 +91,9 @@ class GazeboSystem : public GazeboSystemInterface const hardware_interface::HardwareInfo & hardware_info, gazebo::physics::ModelPtr parent_model); - control_toolbox::Pid extractPID(std::string prefix, - hardware_interface::ComponentInfo joint_info); + control_toolbox::Pid extractPID( + std::string prefix, + hardware_interface::ComponentInfo joint_info); /// \brief Private data class std::unique_ptr dataPtr; diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp index 400bb184..92030e91 100644 --- a/gazebo_ros2_control/src/gazebo_system.cpp +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -34,8 +34,9 @@ struct MimicJoint double multiplier = 1.0; }; -class gazebo_ros2_control::GazeboSystemPrivate{ - public: +class gazebo_ros2_control::GazeboSystemPrivate +{ +public: GazeboSystemPrivate() = default; ~GazeboSystemPrivate() = default; @@ -154,59 +155,61 @@ control_toolbox::Pid GazeboSystem::extractPID( std::string prefix, hardware_interface::ComponentInfo joint_info) { - double kp; - double ki; - double kd; - double max_integral_error; - double min_integral_error; - bool antiwindup; - - if (joint_info.parameters.find(prefix+"kp") != joint_info.parameters.end()) { - kp = std::stod(joint_info.parameters.at(prefix+"kp")); - } else { - kp = 0.0; - } + double kp; + double ki; + double kd; + double max_integral_error; + double min_integral_error; + bool antiwindup; + + if (joint_info.parameters.find(prefix + "kp") != joint_info.parameters.end()) { + kp = std::stod(joint_info.parameters.at(prefix + "kp")); + } else { + kp = 0.0; + } - if (joint_info.parameters.find(prefix+"ki") != joint_info.parameters.end()) { - ki = std::stod(joint_info.parameters.at(prefix+"ki")); - } else { - ki = 0.0; - } + if (joint_info.parameters.find(prefix + "ki") != joint_info.parameters.end()) { + ki = std::stod(joint_info.parameters.at(prefix + "ki")); + } else { + ki = 0.0; + } - if (joint_info.parameters.find(prefix+"kd") != joint_info.parameters.end()) { - kd = std::stod(joint_info.parameters.at(prefix+"kd")); - } else { - kd = 0.0; - } + if (joint_info.parameters.find(prefix + "kd") != joint_info.parameters.end()) { + kd = std::stod(joint_info.parameters.at(prefix + "kd")); + } else { + kd = 0.0; + } - if (joint_info.parameters.find(prefix+"max_integral_error") != joint_info.parameters.end()) { - max_integral_error = std::stod(joint_info.parameters.at(prefix+"max_integral_error")); - } else { - max_integral_error = std::numeric_limits::max(); - } + if (joint_info.parameters.find(prefix + "max_integral_error") != joint_info.parameters.end()) { + max_integral_error = std::stod(joint_info.parameters.at(prefix + "max_integral_error")); + } else { + max_integral_error = std::numeric_limits::max(); + } - if (joint_info.parameters.find(prefix+"min_integral_error") != joint_info.parameters.end()) { - min_integral_error = std::stod(joint_info.parameters.at(prefix+"min_integral_error")); - } else { - min_integral_error = std::numeric_limits::min(); - } + if (joint_info.parameters.find(prefix + "min_integral_error") != joint_info.parameters.end()) { + min_integral_error = std::stod(joint_info.parameters.at(prefix + "min_integral_error")); + } else { + min_integral_error = std::numeric_limits::min(); + } - if (joint_info.parameters.find(prefix+"antiwindup") != joint_info.parameters.end()) { - if (joint_info.parameters.at(prefix+"antiwindup") == "true" || - joint_info.parameters.at(prefix+"antiwindup") == "True") { - antiwindup = true; - } - } else { - antiwindup = false; + if (joint_info.parameters.find(prefix + "antiwindup") != joint_info.parameters.end()) { + if (joint_info.parameters.at(prefix + "antiwindup") == "true" || + joint_info.parameters.at(prefix + "antiwindup") == "True") + { + antiwindup = true; } + } else { + antiwindup = false; + } - RCLCPP_INFO_STREAM(this->nh_->get_logger(), - "setting kp=" << kp - <<" ki=" << ki - <<" kd=" << kd - <<" max_integral_error=" << max_integral_error); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), + "setting kp=" << kp + << " ki=" << ki + << " kd=" << kd + << " max_integral_error=" << max_integral_error); - return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); + return control_toolbox::Pid(kp, ki, kd, max_integral_error, min_integral_error, antiwindup); } void GazeboSystem::registerJoints( @@ -343,18 +346,21 @@ void GazeboSystem::registerJoints( for (unsigned int i = 0; i < joint_info.command_interfaces.size(); i++) { if (joint_info.command_interfaces[i].name == "position" || - joint_info.command_interfaces[i].name == "position_pid") { + joint_info.command_interfaces[i].name == "position_pid") + { if (has_already_registered_pos) { - RCLCPP_ERROR_STREAM(this->nh_->get_logger(), - "can't have position and position" - << "pid command_interface at same time !!!"); + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), + "can't have position and position" + << "pid command_interface at same time !!!"); continue; } has_already_registered_pos = true; - RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " - << joint_info.command_interfaces[i].name); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\t\t " + << joint_info.command_interfaces[i].name); - if (joint_info.command_interfaces[i].name =="position_pid") { + if (joint_info.command_interfaces[i].name == "position_pid") { this->dataPtr->is_pos_pid[j] = true; this->dataPtr->pos_pid[j] = this->extractPID(POSITION_PID_PARAMS_PREFIX, joint_info); } else { @@ -375,16 +381,19 @@ void GazeboSystem::registerJoints( this->dataPtr->sim_joints_[j]->SetPosition(0, initial_position, true); } if (joint_info.command_interfaces[i].name == "velocity" || - joint_info.command_interfaces[i].name == "velocity_pid") { + joint_info.command_interfaces[i].name == "velocity_pid") + { if (has_already_registered_vel) { - RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "can't have velocity and velocity_pid " - << "command_interface at same time !!!"); + RCLCPP_ERROR_STREAM( + this->nh_->get_logger(), "can't have velocity and velocity_pid " + << "command_interface at same time !!!"); continue; } has_already_registered_vel = true; - RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " - << joint_info.command_interfaces[i].name); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\t\t " + << joint_info.command_interfaces[i].name); if (joint_info.command_interfaces[i].name == "velocity_pid") { this->dataPtr->is_vel_pid[j] = true; @@ -598,15 +607,21 @@ GazeboSystem::perform_command_mode_switch( for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { for (const std::string & interface_name : stop_interfaces) { // Clear joint control method bits corresponding to stop interfaces - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { // NOLINT + if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) + { this->dataPtr->joint_control_methods_[j] &= - static_cast(VELOCITY & VELOCITY_PID & EFFORT); + static_cast(VELOCITY & VELOCITY_PID & EFFORT); - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { // NOLINT + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) + { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & POSITION_PID & EFFORT); - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { // NOLINT + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) + { this->dataPtr->joint_control_methods_[j] &= static_cast(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY); } @@ -614,20 +629,25 @@ GazeboSystem::perform_command_mode_switch( // Set joint control method bits corresponding to start interfaces for (const std::string & interface_name : start_interfaces) { - if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) { // NOLINT + if (interface_name == + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_POSITION)) + { if (!this->dataPtr->is_pos_pid[j]) { - this->dataPtr->joint_control_methods_[j] |= POSITION; + this->dataPtr->joint_control_methods_[j] |= POSITION; } else { this->dataPtr->joint_control_methods_[j] |= POSITION_PID; } - } - else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) { // NOLINT + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_VELOCITY)) + { if (!this->dataPtr->is_vel_pid[j]) { this->dataPtr->joint_control_methods_[j] |= VELOCITY; } else { this->dataPtr->joint_control_methods_[j] |= VELOCITY_PID; } - } else if (interface_name == (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) { // NOLINT + } else if (interface_name == // NOLINT + (this->dataPtr->joint_names_[j] + "/" + hardware_interface::HW_IF_EFFORT)) + { this->dataPtr->joint_control_methods_[j] |= EFFORT; } } @@ -694,17 +714,20 @@ hardware_interface::return_type GazeboSystem::write( // set values of all mimic joints with respect to mimicked joint for (const auto & mimic_joint : this->dataPtr->mimic_joints_) { if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & POSITION && - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) { + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & POSITION) + { this->dataPtr->joint_position_cmd_[mimic_joint.joint_index] = mimic_joint.multiplier * this->dataPtr->joint_position_cmd_[mimic_joint.mimicked_joint_index]; } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & VELOCITY && // NOLINT - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) { + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & VELOCITY) + { this->dataPtr->joint_velocity_cmd_[mimic_joint.joint_index] = mimic_joint.multiplier * this->dataPtr->joint_velocity_cmd_[mimic_joint.mimicked_joint_index]; } else if (this->dataPtr->joint_control_methods_[mimic_joint.joint_index] & EFFORT && // NOLINT - this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) { + this->dataPtr->joint_control_methods_[mimic_joint.mimicked_joint_index] & EFFORT) + { this->dataPtr->joint_effort_cmd_[mimic_joint.joint_index] = mimic_joint.multiplier * this->dataPtr->joint_effort_cmd_[mimic_joint.mimicked_joint_index]; } @@ -715,9 +738,9 @@ hardware_interface::return_type GazeboSystem::write( for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { if (this->dataPtr->sim_joints_[j]) { if (this->dataPtr->joint_control_methods_[j] & POSITION) { - double cmd = this->dataPtr->joint_position_cmd_[j]; - this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true); - this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); + double cmd = this->dataPtr->joint_position_cmd_[j]; + this->dataPtr->sim_joints_[j]->SetPosition(0, cmd, true); + this->dataPtr->sim_joints_[j]->SetVelocity(0, 0.0); } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { this->dataPtr->sim_joints_[j]->SetVelocity(0, this->dataPtr->joint_velocity_cmd_[j]); @@ -728,13 +751,13 @@ hardware_interface::return_type GazeboSystem::write( } else if (this->dataPtr->joint_control_methods_[j] & VELOCITY_PID) { double vel_goal = this->dataPtr->joint_velocity_cmd_[j]; double vel = this->dataPtr->sim_joints_[j]->GetVelocity(0); - double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal-vel, dt); + double cmd = this->dataPtr->vel_pid[j].computeCommand(vel_goal - vel, dt); this->dataPtr->sim_joints_[j]->SetForce(0, cmd); } else if (this->dataPtr->joint_control_methods_[j] & POSITION_PID) { double pos_goal = this->dataPtr->joint_position_cmd_[j]; double pos = this->dataPtr->sim_joints_[j]->Position(0); - double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal-pos, dt); + double cmd = this->dataPtr->pos_pid[j].computeCommand(pos_goal - pos, dt); this->dataPtr->sim_joints_[j]->SetForce(0, cmd); } else if (this->dataPtr->is_joint_actuated_[j]) { // Fallback case is a velocity command of zero (only for actuated joints) @@ -752,4 +775,3 @@ hardware_interface::return_type GazeboSystem::write( #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS( gazebo_ros2_control::GazeboSystem, gazebo_ros2_control::GazeboSystemInterface) - \ No newline at end of file diff --git a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py index 2236935e..049537b7 100644 --- a/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py +++ b/gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py @@ -66,7 +66,6 @@ def generate_launch_description(): output='screen' ) - return LaunchDescription([ RegisterEventHandler( event_handler=OnProcessExit(