Skip to content

Commit

Permalink
fix all uncrustify and cpplint error
Browse files Browse the repository at this point in the history
  • Loading branch information
@chameau5050
chameau5050 committed Apr 7, 2024
1 parent d97d839 commit 3987457
Show file tree
Hide file tree
Showing 3 changed files with 104 additions and 82 deletions.
9 changes: 5 additions & 4 deletions gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class GazeboSystemPrivate;

class GazeboSystem : public GazeboSystemInterface
{
public:
public:
// Documentation Inherited
CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info)
override;
Expand Down Expand Up @@ -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);
Expand All @@ -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<GazeboSystemPrivate> dataPtr;
Expand Down
176 changes: 99 additions & 77 deletions gazebo_ros2_control/src/gazebo_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<double>::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<double>::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<double>::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<double>::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(
Expand Down Expand Up @@ -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 {
Expand All @@ -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;
Expand Down Expand Up @@ -598,36 +607,47 @@ 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<ControlMethod_>(VELOCITY & VELOCITY_PID & EFFORT);
static_cast<ControlMethod_>(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<ControlMethod_>(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<ControlMethod_>(POSITION & POSITION_PID & VELOCITY_PID & VELOCITY);
}
}

// 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;
}
}
Expand Down Expand Up @@ -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];
}
Expand All @@ -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]);
Expand All @@ -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)
Expand All @@ -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)

1 change: 0 additions & 1 deletion gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,6 @@ def generate_launch_description():
output='screen'
)


return LaunchDescription([
RegisterEventHandler(
event_handler=OnProcessExit(
Expand Down

0 comments on commit 3987457

Please sign in to comment.