ros2_control - jazzy
Loading...
Searching...
No Matches
inverted_pendulum_tier1_hardware.hpp
1// Copyright 2026 Open Source Robotics Foundation, Inc.
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7// http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15#ifndef INVERTED_PENDULUM_TIER1_HARDWARE__INVERTED_PENDULUM_TIER1_HARDWARE_HPP_
16#define INVERTED_PENDULUM_TIER1_HARDWARE__INVERTED_PENDULUM_TIER1_HARDWARE_HPP_
17
18#include <atomic>
19#include <memory>
20#include <mutex>
21#include <string>
22#include <thread>
23#include <vector>
24
25#include "control_msgs/msg/joint_command.hpp"
26#include "hardware_interface/hardware_info.hpp"
27#include "hardware_interface/system_interface.hpp"
28#include "hardware_interface/types/hardware_interface_return_values.hpp"
29#include "rclcpp/executors/single_threaded_executor.hpp"
30#include "rclcpp/node.hpp"
31#include "rclcpp/publisher.hpp"
32#include "rclcpp/subscription.hpp"
33#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
34#include "rclcpp_lifecycle/state.hpp"
35#include "sensor_msgs/msg/joint_state.hpp"
36
37namespace zenbedded_inverted_pendulum
38{
39
40using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
41
47{
48public:
49 CallbackReturn on_init(
51
52 std::vector<hardware_interface::StateInterface::ConstSharedPtr> on_export_state_interfaces()
53 override;
54 std::vector<hardware_interface::CommandInterface::SharedPtr> on_export_command_interfaces()
55 override;
56
57 CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
58 CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
59
60 hardware_interface::return_type read(
61 const rclcpp::Time & time, const rclcpp::Duration & period) override;
62 hardware_interface::return_type write(
63 const rclcpp::Time & time, const rclcpp::Duration & period) override;
64
65private:
66 void topic_callback(const sensor_msgs::msg::JointState::SharedPtr msg);
67
68 std::string state_topic_{"/zenbedded/inverted_pendulum/state"};
69 std::string command_topic_{"/zenbedded/inverted_pendulum/cmd"};
70
71 std::vector<hardware_interface::StateInterface::SharedPtr> state_interfaces_;
72 std::vector<hardware_interface::CommandInterface::SharedPtr> command_interfaces_;
73
74 // Handles are value-semantics (LooseHandle) and hold their own state; no
75 // extra double-vector staging is needed. write() keeps the last accepted
76 // acceleration command so a NaN command falls back to the previous value.
77 double last_accel_cmd_{0.0};
78
79 std::mutex state_mutex_;
80 double latest_q1_{0.0};
81 double latest_q1_dot_{0.0};
82 double latest_q2_{0.0};
83 double latest_q2_dot_{0.0};
84 std::atomic<bool> has_received_state_{false};
85
86 rclcpp::Node::SharedPtr node_;
87 rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
88 std::thread spin_thread_;
89
90 rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr state_sub_;
91 rclcpp::Publisher<control_msgs::msg::JointCommand>::SharedPtr cmd_pub_;
92};
93
94} // namespace zenbedded_inverted_pendulum
95
96#endif // INVERTED_PENDULUM_TIER1_HARDWARE__INVERTED_PENDULUM_TIER1_HARDWARE_HPP_
Virtual Class to implement when integrating a complex system into ros2_control.
Definition system_interface.hpp:67
Definition inverted_pendulum_tier1_hardware.hpp:47
hardware_interface::return_type read(const rclcpp::Time &time, const rclcpp::Duration &period) override
Read the current state values from the hardware.
Definition inverted_pendulum_tier1_hardware.cpp:200
hardware_interface::return_type write(const rclcpp::Time &time, const rclcpp::Duration &period) override
Write the current command values to the hardware.
Definition inverted_pendulum_tier1_hardware.cpp:217
CallbackReturn on_init(const hardware_interface::HardwareComponentInterfaceParams &params) override
Initialization of the hardware interface from data parsed from the robot's URDF.
Definition inverted_pendulum_tier1_hardware.cpp:38
std::vector< hardware_interface::StateInterface::ConstSharedPtr > on_export_state_interfaces() override
Definition inverted_pendulum_tier1_hardware.cpp:83
std::vector< hardware_interface::CommandInterface::SharedPtr > on_export_command_interfaces() override
Definition inverted_pendulum_tier1_hardware.cpp:95
Parameters required for the initialization of a specific hardware component plugin....
Definition hardware_component_interface_params.hpp:32