adding SCPackRateCommand service to ROS2 Spring Controller

[andermi]

Addresses #44 SCPackRateCommand

Tested via command line. Passes colcon test

Todo:
- [x] SCPackRateCommand via /sc_pack_rate_command

• use ros2 param for sc pack rate

In other PRs:
- [ ] SCResetCommand via /sc_reset_command
- [ ] ValveCommand via /valve_command
- [ ] PumpCommand via /pump_command
- [ ] BenderCommand via /bender_command
- [ ] TetherCommand via /tether_command

[chapulina]

Works for me, I tested with:

ign gazebo mbari_wec.sdf

ros2 topic hz /sc_record

ros2 service call /sc_pack_rate_command buoy_msgs/srv/SCPackRateCommand "rate_hz: 10"

This looks like a good use case for a parameter instead of a service though. That makes it easier to query the current value and set new ones. Not sure if you've considered it.

[andermi]

good use case for a parameter instead of a service

I agree. It would seem I've stumbled into replicating a parameter here. I would use add_on_set_parameters_callback to change the publish rate?

[chapulina]

I would use add_on_set_parameters_callback to change the publish rate?

Yup I think that should work. You can declare the parameter after the node is created and pass it the default value. Something like this:

diff --git a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp
index e2217d1..2ca9f16 100644
--- a/buoy_gazebo/src/controllers/SpringController/SpringController.cpp
+++ b/buoy_gazebo/src/controllers/SpringController/SpringController.cpp
@@ -41,6 +41,7 @@ struct buoy_gazebo::SpringControllerROS2
   rclcpp::Publisher<buoy_msgs::msg::SCRecord>::SharedPtr sc_pub_;
   std::unique_ptr<rclcpp::Rate> pub_rate_;
   buoy_msgs::msg::SCRecord sc_record_;
+  rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr parameter_handler_;
 };
 
 struct buoy_gazebo::SpringControllerServices
@@ -81,6 +82,23 @@ struct buoy_gazebo::SpringControllerPrivate
       rclcpp::init(0, nullptr);
     }
     ros_->node_ = rclcpp::Node::make_shared(node_name, ns);
+    ros_->node_->declare_parameter("publish_rate", 10.0);
+
+    ros_->parameter_handler_ = ros_->node_->add_on_set_parameters_callback(
+     [this](const std::vector<rclcpp::Parameter> & parameters)
+     {
+       rcl_interfaces::msg::SetParametersResult result;
+       result.successful = true;
+        for (const auto & parameter : parameters) {
+          if (
+            parameter.get_name() == "publish_rate" &&
+            parameter.get_type() == rclcpp::PARAMETER_DOUBLE)
+          {
+            // Update rate
+          }
+        }
+        return result;
+      });
 
     ros_->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
     ros_->executor_->add_node(ros_->node_);

Then you can use ros2 param, i.e.

ros2 param get /spring_controller publish_rate

[andermi]

@chapulina if you saw my previous comments, I figured it out... PEBKAC 🤦‍♂️

[andermi]

@hamilton8415 , do we want to model the relief valve or pump? If so, do you have data on pressure/flow rates of either?

[chapulina]

Thanks for iterating, works well for me with the parameters. How I tested:

ign gazebo mbari_wec.sdf

ros2 topic hz /sc_record

ros2 param set /spring_controller publish_rate 10.0

[andermi]

I started adding valve command stuff but realized I should do that in a new PR 😜