From c65412ce26662787cc7f10723cd53855c8a3c05e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 3 Nov 2024 11:13:10 +0000 Subject: [PATCH] Ex7 --- example_7/doc/userdoc.rst | 37 +++----- .../r6bot_hardware.hpp | 14 +-- example_7/hardware/r6bot_hardware.cpp | 88 ++++--------------- .../reference_generator/send_trajectory.cpp | 11 +++ 4 files changed, 42 insertions(+), 108 deletions(-) diff --git a/example_7/doc/userdoc.rst b/example_7/doc/userdoc.rst index 50402591d..c2a4e6b58 100644 --- a/example_7/doc/userdoc.rst +++ b/example_7/doc/userdoc.rst @@ -193,13 +193,14 @@ In ros2_control, hardware system components are integrated via user defined driv The following code blocks will explain the requirements for writing a new hardware interface. -The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement five public methods. +The hardware plugin for the tutorial robot is a class called ``RobotSystem`` that inherits from ``hardware_interface::SystemInterface``. The ``SystemInterface`` is one of the offered hardware interfaces designed for a complete robot system. For example, The UR5 uses this interface. The ``RobotSystem`` must implement the following public methods. 1. ``on_init`` -2. ``export_state_interfaces`` -3. ``export_command_interfaces`` -4. ``read`` -5. ``write`` +2. ``on_configure`` +3. ``read`` +4. ``write`` + +There are more methods that can be implemented for lifecycle changes, but they are not required for the tutorial robot. .. code-block:: c++ @@ -209,15 +210,14 @@ The hardware plugin for the tutorial robot is a class called ``RobotSystem`` tha class HARDWARE_INTERFACE_PUBLIC RobotSystem : public hardware_interface::SystemInterface { public: CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; - std::vector export_state_interfaces() override; - std::vector export_command_interfaces() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; // private members // ... } -The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. Instead, vectors will be initialized that represent the state all the hardware, e.g. a vector of doubles describing joint angles, etc. +The ``on_init`` method is called once during ros2_control initialization if the ``RobotSystem`` was specified in the URDF. In this method, communication between the robot hardware needs to be setup and memory dynamic should be allocated. Since the tutorial robot is simulated, explicit communication will not be established. .. code-block:: c++ @@ -232,26 +232,15 @@ The ``on_init`` method is called once during ros2_control initialization if the Notably, the behavior of ``on_init`` is expected to vary depending on the URDF file. The ``SystemInterface::on_init(info)`` call fills out the ``info`` object with specifics from the URDF. For example, the ``info`` object has fields for joints, sensors, gpios, and more. Suppose the sensor field has a name value of ``tcp_force_torque_sensor``. Then the ``on_init`` must try to establish communication with that sensor. If it fails, then an error value is returned. -Next, ``export_state_interfaces`` and ``export_command_interfaces`` methods are called in succession. The ``export_state_interfaces`` method returns a vector of ``StateInterface``, describing the ``state_interfaces`` for each joint. The ``StateInterface`` objects are read only data handles. Their constructors require an interface name, interface type, and a pointer to a double data value. For the ``RobotSystem``, the data pointers reference class member variables. This way, the data can be access from every method. - -.. code-block:: c++ - - std::vector RobotSystem::export_state_interfaces() { - std::vector state_interfaces; - // add state interfaces to ``state_interfaces`` for each joint, e.g. `info_.joints[0].state_interfaces_`, `info_.joints[1].state_interfaces_`, `info_.joints[2].state_interfaces_` ... - // ... - return state_interfaces; - } - -The ``export_command_interfaces`` method is nearly identical to the previous one. The difference is that a vector of ``CommandInterface`` is returned. The vector contains objects describing the ``command_interfaces`` for each joint. +The ``on_configure`` method is called once during ros2_control lifecycle of the ``RobotSystem``. Initial values are set for state and command interfaces that represent the state all the hardware, e.g. doubles describing joint angles, etc. .. code-block:: c++ - std::vector RobotSystem::export_command_interfaces() { - std::vector command_interfaces; - // add command interfaces to ``command_interfaces`` for each joint, e.g. `info_.joints[0].command_interfaces_`, `info_.joints[1].command_interfaces_`, `info_.joints[2].command_interfaces_` ... + CallbackReturn RobotSystem::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) + // setup communication with robot hardware // ... - return command_interfaces; + return CallbackReturn::SUCCESS; } The ``read`` method is core method in the ros2_control loop. During the main loop, ros2_control loops over all hardware components and calls the ``read`` method. It is executed on the realtime thread, hence the method must obey by realtime constraints. The ``read`` method is responsible for updating the data values of the ``state_interfaces``. Since the data value point to class member variables, those values can be filled with their corresponding sensor values, which will in turn update the values of each exported ``StateInterface`` object. diff --git a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp index 17688a80b..69da3d74f 100644 --- a/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp +++ b/example_7/hardware/include/ros2_control_demo_example_7/r6bot_hardware.hpp @@ -36,25 +36,13 @@ class RobotSystem : public hardware_interface::SystemInterface public: CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; - std::vector export_state_interfaces() override; - - std::vector export_command_interfaces() override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; protected: - /// The size of this vector is (standard_interfaces_.size() x nr_joints) - std::vector joint_position_command_; - std::vector joint_velocities_command_; - std::vector joint_position_; - std::vector joint_velocities_; - std::vector ft_states_; - std::vector ft_command_; - - std::unordered_map> joint_interfaces = { - {"position", {}}, {"velocity", {}}}; }; } // namespace ros2_control_demo_example_7 diff --git a/example_7/hardware/r6bot_hardware.cpp b/example_7/hardware/r6bot_hardware.cpp index 7414b8fcf..4f710eb64 100644 --- a/example_7/hardware/r6bot_hardware.cpp +++ b/example_7/hardware/r6bot_hardware.cpp @@ -16,6 +16,8 @@ #include #include +#include + namespace ros2_control_demo_example_7 { CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & info) @@ -24,95 +26,39 @@ CallbackReturn RobotSystem::on_init(const hardware_interface::HardwareInfo & inf { return CallbackReturn::ERROR; } - - // robot has 6 joints and 2 interfaces - joint_position_.assign(6, 0); - joint_velocities_.assign(6, 0); - joint_position_command_.assign(6, 0); - joint_velocities_command_.assign(6, 0); - - // force sensor has 6 readings - ft_states_.assign(6, 0); - ft_command_.assign(6, 0); - - for (const auto & joint : info_.joints) - { - for (const auto & interface : joint.state_interfaces) - { - joint_interfaces[interface.name].push_back(joint.name); - } - } - - return CallbackReturn::SUCCESS; + return hardware_interface::CallbackReturn::SUCCESS; } -std::vector RobotSystem::export_state_interfaces() +CallbackReturn RobotSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { - std::vector state_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) + // reset values always when configuring hardware + for (const auto & [name, descr] : joint_state_interfaces_) { - state_interfaces.emplace_back(joint_name, "position", &joint_position_[ind++]); + set_state(name, 0.0); } - - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) + for (const auto & [name, descr] : joint_command_interfaces_) { - state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]); + set_command(name, 0.0); } - - state_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_states_[0]); - state_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_states_[1]); - state_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_states_[2]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_states_[3]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_states_[4]); - state_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_states_[5]); - - return state_interfaces; -} - -std::vector RobotSystem::export_command_interfaces() -{ - std::vector command_interfaces; - - int ind = 0; - for (const auto & joint_name : joint_interfaces["position"]) - { - command_interfaces.emplace_back(joint_name, "position", &joint_position_command_[ind++]); - } - - ind = 0; - for (const auto & joint_name : joint_interfaces["velocity"]) + for (const auto & [name, descr] : sensor_state_interfaces_) { - command_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_command_[ind++]); + set_state(name, 0.0); } - command_interfaces.emplace_back("tcp_fts_sensor", "force.x", &ft_command_[0]); - command_interfaces.emplace_back("tcp_fts_sensor", "force.y", &ft_command_[1]); - command_interfaces.emplace_back("tcp_fts_sensor", "force.z", &ft_command_[2]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.x", &ft_command_[3]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.y", &ft_command_[4]); - command_interfaces.emplace_back("tcp_fts_sensor", "torque.z", &ft_command_[5]); - - return command_interfaces; + return CallbackReturn::SUCCESS; } return_type RobotSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // TODO(pac48) set sensor_states_ values from subscriber - for (auto i = 0ul; i < joint_velocities_command_.size(); i++) - { - joint_velocities_[i] = joint_velocities_command_[i]; - joint_position_[i] += joint_velocities_command_[i] * period.seconds(); - } - - for (auto i = 0ul; i < joint_position_command_.size(); i++) + for (std::size_t i = 0; i < info_.joints.size(); i++) { - joint_position_[i] = joint_position_command_[i]; + const auto name_vel = info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY; + const auto name_pos = info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION; + set_state(name_vel, get_command(name_vel)); + set_state(name_pos, get_state(name_pos) + get_state(name_vel) * period.seconds()); } - return return_type::OK; } diff --git a/example_7/reference_generator/send_trajectory.cpp b/example_7/reference_generator/send_trajectory.cpp index e6d3fd787..c9d609d6e 100644 --- a/example_7/reference_generator/send_trajectory.cpp +++ b/example_7/reference_generator/send_trajectory.cpp @@ -96,6 +96,17 @@ int main(int argc, char ** argv) trajectory_msg.points.push_back(trajectory_point_msg); } + // send zero velocities in the end + std::memset( + trajectory_point_msg.velocities.data(), 0.0, + trajectory_point_msg.velocities.size() * sizeof(double)); + trajectory_point_msg.time_from_start.sec = trajectory_len / loop_rate; + trajectory_point_msg.time_from_start.nanosec = static_cast( + 1E9 / loop_rate * + static_cast( + trajectory_len - loop_rate * (trajectory_len / loop_rate))); // implicit integer division + trajectory_msg.points.push_back(trajectory_point_msg); + pub->publish(trajectory_msg); while (rclcpp::ok()) {