getting ready for PR

Author: evan-palmer

auv_control_demos/chained_controllers/README.md

@@ -88,7 +88,7 @@ applies a PWM command to the hardware interface.
    following command:
 
    ```bash
-   ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
+   ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry
    ```
 
 5. The ISMC accepts reference commands sent over a topic or a reference

auv_control_demos/individual_controller/README.md

@@ -1,7 +1,6 @@
 # Example 1: Individual Controller
 
-This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how
-to launch a single controller.
+This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how to launch a single controller.
 
 ## Tutorial Steps
 
@@ -62,7 +61,7 @@ to launch a single controller.
    topic-based interface:
 
    ```bash
-   ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
+   ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry
    ```
 
 5. The ISMC accepts reference commands sent via a topic or the controller's
@@ -80,7 +79,7 @@ to launch a single controller.
    ros2 topic echo /integral_sliding_mode_controller/status
    ```
 
-   Should yield an output similar to the following:
+   should yield an output similar to the following:
 
    ```bash
    header:

auv_control_msgs/CMakeLists.txt

@@ -6,10 +6,12 @@ find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(trajectory_msgs REQUIRED)
 
 rosidl_generate_interfaces(auv_control_msgs
   "msg/MultiActuatorStateStamped.msg"
-  DEPENDENCIES builtin_interfaces std_msgs geometry_msgs
+  "msg/IKControllerStateStamped.msg"
+  DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
 )
 
 ament_package()

auv_control_msgs/msg/IKControllerStateStamped.msg

@@ -0,0 +1,20 @@
+std_msgs/Header header
+
+# The name of the IK solver used by the controller
+string solver_name
+
+# Position DoF names, e.g., joint or axis names.
+string[] position_joint_names
+
+# Velocity DoF names.
+string[] velocity_joint_names
+
+# The reference end effector pose.
+geometry_msgs/Pose reference
+
+# Time between two consecutive updates/execution of the control law.
+# This is often used for integration of the solution to determine the desired positions.
+float64 time_step
+
+# The IK solver solution.
+trajectory_msgs/JointTrajectoryPoint solution

auv_control_msgs/package.xml

@@ -22,6 +22,8 @@
   <test_depend>ament_lint_common</test_depend>
 
   <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>trajectory_msgs</depend>
 
   <exec_depend>rosidl_default_runtime</exec_depend>
   <member_of_group>rosidl_interface_packages</member_of_group>

velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp

@@ -329,7 +329,7 @@ auto AdaptiveIntegralTerminalSlidingModeController::update_and_write_commands(
 
   if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) {
     rt_controller_state_pub_->msg_.header.stamp = time;
-    for (const auto && [i, state] : std::views::enumerate(rt_controller_state_pub_->msg_.dof_states)) {
+    for (auto && [i, state] : std::views::enumerate(rt_controller_state_pub_->msg_.dof_states)) {
       const auto out = command_interfaces_[i].get_optional();
       state.reference = reference_interfaces_[i];
       state.feedback = system_state_values_[i];

velocity_controllers/src/integral_sliding_mode_controller.cpp

@@ -308,7 +308,7 @@ auto IntegralSlidingModeController::update_and_write_commands(
 
   if (rt_controller_state_pub_ && rt_controller_state_pub_->trylock()) {
     rt_controller_state_pub_->msg_.header.stamp = time;
-    for (const auto && [i, state] : std::views::enumerate(rt_controller_state_pub_->msg_.dof_states)) {
+    for (auto && [i, state] : std::views::enumerate(rt_controller_state_pub_->msg_.dof_states)) {
       const auto out = command_interfaces_[i].get_optional();
       state.reference = reference_interfaces_[i];
       state.feedback = system_state_values_[i];

whole_body_controllers/CMakeLists.txt

@@ -24,6 +24,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
     message_transforms
     controller_common
     ik_solvers
+    auv_control_msgs
 )
 
 foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})

whole_body_controllers/include/whole_body_controllers/ik_controller.hpp

@@ -20,6 +20,7 @@
 
 #pragma once
 
+#include "auv_control_msgs/msg/ik_controller_state_stamped.hpp"
 #include "controller_interface/chainable_controller_interface.hpp"
 #include "controller_interface/controller_interface.hpp"
 #include "geometry_msgs/msg/pose.hpp"
@@ -94,6 +95,10 @@ class IKController : public controller_interface::ChainableControllerInterface
   std::unique_ptr<ik_controller::ParamListener> param_listener_;
   ik_controller::Params params_;
 
+  std::shared_ptr<rclcpp::Publisher<auv_control_msgs::msg::IKControllerStateStamped>> controller_state_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<auv_control_msgs::msg::IKControllerStateStamped>>
+    rt_controller_state_pub_;
+
   // make the free-flyer position and velocity dof names "static"
   std::vector<std::string> free_flyer_pos_dofs_{"x", "y", "z", "qx", "qy", "qz", "qw"};
   std::vector<std::string> free_flyer_vel_dofs_{"x", "y", "z", "rx", "ry", "rz"};

whole_body_controllers/package.xml

@@ -29,6 +29,7 @@
   <depend>message_transforms</depend>
   <depend>controller_common</depend>
   <depend>ik_solvers</depend>
+  <depend>auv_control_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>