ros-controls / control_toolbox Goto Github PK

I think this happens when loading new controllers.

pr2_ethercat: /usr/include/boost/smart_ptr/shared_ptr.hpp:418: T* boost::shared_ptr<T>::operator->() const [with T = dynamic_reconfigure::Server<control_toolbox::ParametersConfig>]: Assertion `px != 0' failed.

Something is still off with our codecov setup, getting some strange errors.
The secret is set in the repo.
#97

There should be a clear math class and the interface class around it. I just had to fix some weird test errors arising from mess we've been gathering there.

Related PR #54

I'm looking into how much work it would be to port this package into ROS 2 Dashing (looks like realtime_tools needs to be ported first).

• Is someone already working on this?
• Where should a ros2 port live? How about a dashing-devel branch in this repo?
• Would the maintainers be willing to release into Dashing once code has been ported?
• Should the existing code style be maintained, or should the ROS 2 style guide be used?
• Should deprecated methods be kept or removed?

  • control_toolbox/include/control_toolbox/pid.h

    Line 329 in 87c3ca1

    ROS_DEPRECATED double updatePid(double p_error, ros::Duration dt);

Currently all the branches in the control_toolbox still use the old configuration (without docker) that means that all building is done using hydro (even for the kinetic branch).

Although it works at the moment, we really should update it as various issues might arise and go unnoticed (all builds are tested using ubuntu 12.04 which is quite old).

I can make a PR with a fix if the maintainers agree as well.

Friendly ping to @bmagyar. I've been going over dependencies and realized this package has no rolling release yet. ros2_controllers depends on this for a rolling release.

Blocked by ros-controls/realtime_tools#74

Hello,

I am working on a some kind of adaptive PID control strategy where I need to set the PID gains fast. However the update of the dynamic reconfigure takes too much time, making my controller to not behave as it should.

https://github.com/ros-controls/control_toolbox/blob/kinetic-devel/src/pid.cpp#L241

Would it be possible to provide a function without the update dynamic reconfigure functionality?

Thank you.

Done.
ros/rosdistro#4226

• add melodic
• remove lunar

We have a compilation issue on RHEL 8 with eigen:
https://github.com/ros-controls/control_toolbox/actions/runs/7784307246/job/21224592892?pr=177

Maybe we should consider linking against it as outlined here
PickNikRobotics/RSL#114

Should be set up for the ros2-master branch

I tried to build control_toolbox on Windows (MSVC) (using colcon build)
It gives me this LNK1181 error.

Building Custom Rule C:/Program Files/ros2_ws/src/control_toolbox/CMakeLists.txt
test_load_low_pass_filter.cpp                                              
LINK : fatal error LNK1181: �޷��������ļ���Release\low_pass_filter.lib��[C:\ProgramFiles\ros2_ws\build\control_toolbox\test_load_low_pass_filter.vcxproj]
Building Custom Rule C:/Program Files/ros2_ws/src/control_toolbox/CMakeLists.txt                                                                                                               
test_low_pass_filter.cpp
[Processing: control_toolbox]                                                                          
LINK : fatal error LNK1181: �޷��������ļ���Release\low_pass_filter.lib��[C:\ProgramFiles\ros2_ws\build\control_toolbox\test_low_pass_filter.vcxproj]                                                                                                           
Failed   <<< control_toolbox [1min 3s, exited with code 1]   

Anyone knows how to fix this?

Hello,

I'm using dynamic_reconfigure to tune some PID's in Gazebo through this nice class.
The i_clamp_min bounds are set to [0,1000], however it should be the other way around : [-1000,0].

At the Pid.init, i_clamp_min is set to -i_clamp so everything is fine.
As soon as rqt_gui (for instance) is used to display the gains, the minimum value is applied and thus i_clamp_min is set to 0.

This could be changed in cfg/Parameters.cfg.

We just had a lengthy debugging session because we uploaded "i_clamp_min" and "i_clamp_max" to the parameter server (after using dynamic_reconfigure with these params for a while and also looking up the dynamic_reconfigure config, seeing these two parameters). Strangely, the parameters somehow always defaulted to 0 when running our controllers, no matter what value we set them to.

Turns out, on startup, only a "i_clamp" parameter is read from the parameter server. This was not set in our setup, so it defaulted to 0, which triggered setting "i_clamp_min" and "i_clamp_max" symmetrically (to -0.0/0.0) on the parameter server, too.

Would it make sense to also check for the existence of "i_clamp_min" and "i_clamp_max" on the parameter first on init? Appears this wouldn´t break existing code, but allow using the parameters without running into this annoyance.

Looks like since Kinetic the equations aren't rendering in the doxygen docs:

• http://docs.ros.org/kinetic/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html#ab3659453dbdf1b4261cc56f4cb2f5bf1
• http://docs.ros.org/lunar/api/control_toolbox/html/classcontrol__toolbox_1_1Pid.html#ab3659453dbdf1b4261cc56f4cb2f5bf1

Still using rosbuild (haven't fully converted to catkin yet), getting this error:

In file included from /home/dinh/groovy_workspace/control_toolbox/include/control_toolbox/pid_gains_setter.h:40:0,
from /home/dinh/groovy_workspace/control_toolbox/src/pid_gains_setter.cpp:30:
/home/dinh/groovy_workspace/control_toolbox/include/control_toolbox/pid.h:43:46: fatal error: control_toolbox/ParametersConfig.h: No such file or directory
compilation terminated.
In file included from /home/dinh/groovy_workspace/control_toolbox/src/pid.cpp:41:0:
/home/dinh/groovy_workspace/control_toolbox/include/control_toolbox/pid.h:43:46: fatal error: control_toolbox/ParametersConfig.h: No such file or directory
compilation terminated.

          Unless the filter_base is changed, I don't think the filter has any access to the node (only its logger and parameter interfaces). Maybe the filter should get a time and dt passed when updated (as controllers do)

Originally posted by @guihomework in #153 (comment)

There are 2 options I can see right now:

1. Change FilterBase and FilterChainBase to support this argument in the configure()
2. Add override of the FilterBase and FilterChainBase classes and enable controllers to use it instead of the generic filters.

• The very first line in the readme of the ros2-master branch points to non-ROS 2 docs
• Maybe add/link some demos as proposed in #70. I know that one can use the tests to figure out how to use the functions, but that's not very userfriendly imho ;)

What do you think?

When we add other filters often used in controllers, we should also move exponentialSmoothing in a filter to make its use clearer.

Since we are using normal dynamic reconfigure the min and max values for each parameter are hard coded in the cfg file like so:
gen.add( "p" , double_t, 1, "Proportional gain.", 10.0, -100000, 100000).
With the 2nd last variable representing the minimum value and the last value representing the maximum value.

Since the default values are -100 000 to 100 000, controllers that otherwise need gain values orders of magnitude less can't be tuned using the dynamic reconfigure gui. Is there an elegant way we can pass the min and max values for each gain through the constructor (optionally) and then have them updated on the dynamic reconfigure server for visibility on clients? I can't seem to find a way for clients to modify these values themselves.

| [Processing: control_toolbox]
| --- stderr: control_toolbox
| Errors while running CTest
| Output from these tests are in: /home/christoph/projects/ros2_rolling_ws/src/control_toolbox/build/control_toolbox/Testing/Temporary/LastTest.log
| Use "--rerun-failed --output-on-failure" to re-run the failed cases verbosely.
| ---
| Finished <<< control_toolbox [1min 3s]	[ with test failures ]
| 
| Summary: 1 package finished [1min 5s]
|   1 package had stderr output: control_toolbox
|   1 package had test failures: control_toolbox
| build/control_toolbox/Testing/20240211-2118/Test.xml: 5 tests, 0 errors, 1 failure, 0 skipped
| - test_low_pass_filter
|   <<< failure message
|     -- run_test.py: invoking following command in '/home/christoph/projects/ros2_rolling_ws/src/control_toolbox/build/control_toolbox':
|      - /home/christoph/projects/ros2_rolling_ws/src/control_toolbox/build/control_toolbox/test_low_pass_filter --ros-args --params-file /home/christoph/projects/ros2_rolling_ws/src/control_toolbox/src/ros-controls/control_toolbox/test/control_filters/test_low_pass_filter_parameters.yaml -- --gtest_output=xml:/home/christoph/projects/ros2_rolling_ws/src/control_toolbox/build/control_toolbox/test_results/control_toolbox/test_low_pass_filter.gtest.xml
|     [==========] Running 4 tests from 1 test suite.
|     [----------] Global test environment set-up.
|     [----------] 4 tests from LowPassFilterTest
|     [ RUN      ] LowPassFilterTest.TestLowPassWrenchFilterAllParameters
|     [WARN] [1707686297.315944193] [TestLowPassWrenchFilterAllParameters]: Filter TestLowPassFilter already being reconfigured
|   >>>
| build/control_toolbox/test_results/control_toolbox/test_low_pass_filter.gtest.xml: 1 test, 1 error, 0 failures, 0 skipped
| - control_toolbox test_low_pass_filter.gtest.missing_result
|   <<< error message
|     The test did not generate a result file.
|   >>>
| 
| Summary: 32 tests, 1 error, 1 failure, 0 skipped
[debian_source_build-3/Reusable Debian Source Build/humble debian build] Failed but continue next step
[debian_source_build-3/Reusable Debian Source Build/humble debian build]   ❌  Failure - Main Test workspace

can be locally reproduced with gh act pull_request -W .github/workflows/debian-build.yml

An automated scheduled build failed on ros2-master: https://github.com/ros-controls/control_toolbox/actions/runs/9477893138

Even though i_term is clamped to i_max, i_error will continue to windup as long as there's an error. I've found this troublesome in an application with quadrotor control, where I'm trying to migrate the bespoke PID methods to the control_toolbox ones.

Would you be open to a pull adding a "antiwindup" parameter to enable clamping i_error by i_max/i_gain_?

Line 342 of pid.cpp

If an error or dt=0, occurs you return zero but don't update the stored command.
I saw this causing issues in simulation

I suggest the following:

  if (dt == ros::Duration(0.0))
  {
      return cmd_;
  }
  else if (std::isnan(error) || std::isinf(error) || std::isnan(error_dot) || std::isinf(error_dot))
  {
    i_term_ = 0.0;
    cmd_=0.0;
    return 0.0;
  }

I haven't pushed change to our fork yet, so just putting this up for discussion.

Hi, im on ubuntu 22, ros rolling.

I cannot manage to make the RosPid class work.
The Pid class itself works, but when I use RosPid like this:

control_toolbox::PidROS*  _x_pid_a;   
_x_pid_a = new control_toolbox::PidROS(this->shared_from_this(),"x_pid");   
_x_pid_a->initPid();   
_x_pid_a->reset();   
auto dt = rclcpp::Duration(50ms)   
x_cmd = _x_pid_a->computeCommand(x_err,dt);   

the node crashes, even if all the parameters are correctly given to the node.
Gdb blames line 138 in src/pid.cpp:

error_dot_ = d_error_;

seems as if the underneath pid class is not well initialized.
Am I missing something?

...and create a new RosPid in a separate package that offers parameterization and services through the node interface. This would wrap the features now offered between the pid_gains_setter and the stuff coupled inside Pid::init*() functions

Split from #83

The pid_tests target has been broken for a while now, and it fails with the following message:

control_toolbox/test/pid_tests.cpp:170:33: error: passing ‘const control_toolbox::Pid’ as ‘this’ argument of ‘control_toolbox::Pid::Gains control_toolbox::Pid::getGains()’ discards qualifiers [-fpermissive]

The offending code is:

const Pid pid4(pid3);
Pid::Gains g3 = pid4.getGains();

It's calling the non-const method getGains() of the const object pid4.

Does it actually make sense to have a const Pid instance?. It would seem that you can't do much with it anyway. If there is indeed need for accessing the gains from a const instance, an API addition would be required. I tested this and it works (tests pass), but is pretty ugly:

const Pid::Gains& Pid::getGains() const
{
  return *(const_cast<realtime_tools::RealtimeBuffer<Gains>& >(gains_buffer_).readFromRT());
}

Didn't take the time to see if a cleaner implementation is possible. Still, I'd avoid it if there is no real need.

@davetcoleman could you take a look at it?. You wrote that part of the test and worked on the pid implementation back in July.

Generally agree with this changes!

I would open as followup an issue about renaming topic_prefix_ because its name is misleading, parameter_perfix_ would be more suitable.

Originally posted by @destogl in #119 (review)

When attempting to build in same catkin workspace as sensor_msgs, build fails because control_toolbox doesn't explicitly list sensor_msgs as a dependency.

fix:

package.xml
add < build_depend >sensor_msgs</ build_depend >
and < run_depend >sensor_msgs< /run_depend >

CMakeLists.xml
find_package(catkin REQUIRED COMPONENTS
...
sensor_msgs
)

gazebo_ros_control depends on this and it's part of "desktop-full", so it would be nice to have an initial version of this out as soon as possible.

Thanks in advanced.

I am working now with PID controller PidROS class and it is a bit confusing that nested parameters are not supported properly.

I would like to define my parameters as follows:

pids:
  x: {p: 1, d: 1, i: 1}
  y: {p: 1, d: 1, i: 1}
  ...

This is OK if I parse parameters directly, but if I would like to get them using PidROS class I would have to construct it with PidROS( node, "pids.x"), but this is not possible since a ROS topic can not have "." in the name. The structure is expecting to use ROS1 syntax for parameters with "/", e.g. PidROS( node, "pids/x"), but this is not valid anymore in ROS2.

I have two proposals to solve this issue:

1. Extend constructor with parameter_prefix argument separating topic's prefix and parameters' prefix completely, this would result in the constructor like: PidROS(node, "pids/x", "pids.x").

2. Use one prefix, but replace all "/" with "." for parameters, then we could use the constructor for the above example: PidROS(node, "pids/x"), where on the fly "calculated" parameters' prefix would be "pids.x"

What do you think?

Hello,

I made an example package/tutorial for the Pid class:

https://github.com/awesomebytes/control_toolbox_pid_tutorial

I find it a very reusable object (who wants to reimplement PIDs?) but I found it lacking documentation, as it took me a while to figure out how it actually works, specially, how to get the dynamic reconfigure running.

Let me know if you want to add that tutorial somwhere else. You may want to mention it on the README otherwise. Also, I'm far from a c++ expert, so from that point of view you may find weird/ugly stuff, I'm sure ;)

Downstream packages like gazebo_ros_control are using the changes added in #22. Releasing control_toolbox is a requisite for releasing these packages, and also to have clean continuous integration of the development branches.

Since #22 adds functionality in a backwards-compatible manner, a minor version bump from 1.11 to 1.12 would be required.

Before creating any pull request, I'd like to raise this for discussion.

Lines 282 and 315 in pid.cpp, explicitly:

i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_;

Shouldn't this term be:

i_term_ = gains.i_gain_*( i_term_ +  dt.toSec() * p_error_ );

Note that, for instance, setting gains.i_gain_ to zero online in the current implementation does not remove the integrated i_term_ so far, and it won't until you reset the controller.

/cc @manuelbonilla

Last one was over a year ago— would be great to get another tag, and blooms for kinetic + lunar.

Needed for ros-controls/ros_controllers#212

...to prevent situations such as those reported in #40.

@paulbovbel, @carlosjoserg

Using this toolbox, I was confused by the order of arguments in the constructor:

    Gains(double p, double i, double d, double i_max, double i_min)
       : p_gain_(p),
         i_gain_(i),
         d_gain_(d),
         i_max_(i_max),
         i_min_(i_min),
         antiwindup_(false)
     {}

It would be nice to throw an error if i_min >= i_max

After upgrading today I started my robot (KUKA LWR4+ with effort joint_trajectory_controller and MoveIt) and it immediately started going at full speed without any reason. I could observe this behavior consistently until I downgraded control_toolbox to 1.13.0 (it tried to reach the same position again and again at full speed, without any care for the joint limits).

This is very dangerous (I had my hand on the red button by pure chance, otherwise I would have damaged my robot very heavily).

Am I the only one with this problem?

@carlosjoserg, I see there is a commit of yours there. Didn't you have any problems with this? Strange, since we have the same setup.

Split from #83

After #15 is merged we should remove all the rosmake/rosbuild artifacts from the indigo branch. Reminder to self.

There is a working version on the ros2-master branch already but none of it is released AFAIK.

This probably blocks the creation of position_controllers, velocity_controllers and effort_controllers.

There is an option to move control_toolbox into ros2_control or ros2_controllers if you prefer it that way, it did not get much use released as an independent package.

As far as I read it is not yet possible with ros-controls to define different controllers other than a PID controller, that is to say, you can not choose for example for an lowpass filter in combination with a notch filter (or lead-lag filter, or any other standard type of filters) and choose different discretization schemes, am I correct? Is this because no one needs them? Otherwise, I might be willing to start implementing these kind of controllers.

I wanted to use LowPassFilter, but it seems to me that there is no way to pass parameters via API (like PID constructor or setGains() call).

Wouldn't be better to make LowPassFilter support parameter passing via API and a corresponding LowPassFilterROS querying parameters from configuration instead?
Or just modifying LowPassFilter to support both scenarios...

I would contribute a PR following your feedbacks of course.

could you please merge the files here too as in ros2_control?

Originally posted by @bmagyar in #110 (comment)

...

The copy-paste files on different distros should ideally be in the same file

I was switching my code, which uses the PID controller from this project, from ROS1 to ROS2 and ran into the following issue:
When the PID controller is created a reference on the node is taken as a parameter. I assumed that this should be a reference to the node that is using this PID controller. The description of the constructor (https://github.com/ros-controls/control_toolbox/blob/ros2-master/include/control_toolbox/pid_ros.hpp#L58-L61) seemed to suggest that I only need to put it in a correct namespace.
Therefore, I programmed something like this:

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("node");
node->declare_parameter<double>("pid.p", 0.0);
node->declare_parameter<double>("pid.i", 0.0);
node->declare_parameter<double>("pid.d", 0.0);
node->declare_parameter<double>("pid.i_clamp_max", 0.0);
node->declare_parameter<double>("pid.i_clamp_min", 0.0);
node->declare_parameter<bool>("pid.antiwindup", false);
control_toolbox::PidROS pid = control_toolbox::PidROS(node, "pid");
pid.initPid();
node->declare_parameter<double>("not_related_to_pid", 0.0);
double not_related_to_pid;
node->get_parameter("not_related_to_pid", not_related_to_pid);

Executing this leads to following error:

[Node-4] terminate called after throwing an instance of 'rclcpp::exceptions::InvalidParameterValueException'
[Node-4]   what():  parameter 'not_related_to_pid' could not be set: Invalid parameter

I think, the reason is that the pid controller sets the add_on_set_parameters_callback here:

I managed to fix this by creating a seperate node for the pid controller.

std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("node");
std::shared_ptr<rclcpp::Node> node_pid = rclcpp::Node::make_shared("pid");
node_pid->declare_parameter<double>("p", 0.0);
node_pid->declare_parameter<double>("i", 0.0);
node_pid->declare_parameter<double>("d", 0.0);
node_pid->declare_parameter<double>("i_clamp_max", 0.0);
node_pid->declare_parameter<double>("i_clamp_min", 0.0);
node_pid->declare_parameter<bool>("antiwindup", false);
control_toolbox::PidROS pid = control_toolbox::PidROS(node_pid, "");
pid.initPid();
node->declare_parameter<double>("not_related_to_pid", 0.0);
double not_related_to_pid;
node->get_parameter("not_related_to_pid", not_related_to_pid);

My question is now: Is this intended behavior? Should every PID controller have its own node?
If yes, I think the description of the constructor should be improved or a clearer error message should be thrown to point out that it needs it own node.

Additionally, I needed to declare the parameters of the controller before calling pid.initPid() to load yaml parameters from a launch file correctly. I think this is because the PID controller only calls the decleration

We are using "control_toolbox" as a library in our project and we experience unexpected compilation errors between version 2.1 and 2.2 in ubuntu 22.04.

Basically due to a change in "CMakeList.txt" in install section introduced in commit : 9faaa68
the control_toolbox headers files are installed in /opt/ros/humble/include/control_toolbox/control_toolbox/ for version 2.2.0 and for recent version 2.1.2 was installed /opt/ros/humble/include/control_toolbox/.
Is this change intentional?

Context
The original issue is here: o3de/o3de-extras#198.