ros2_control - rolling
joint_state_broadcaster.hpp
1 // Copyright 2017 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 JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
16 #define JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
17 
18 #include <memory>
19 #include <string>
20 #include <unordered_map>
21 #include <vector>
22 
23 #include "control_msgs/msg/dynamic_joint_state.hpp"
24 #include "controller_interface/controller_interface.hpp"
25 #include "joint_state_broadcaster/visibility_control.h"
26 // auto-generated by generate_parameter_library
27 #include "joint_state_broadcaster_parameters.hpp"
28 #include "realtime_tools/realtime_publisher.hpp"
29 #include "sensor_msgs/msg/joint_state.hpp"
30 #include "urdf/model.h"
31 
32 namespace joint_state_broadcaster
33 {
59 {
60 public:
61  JOINT_STATE_BROADCASTER_PUBLIC
63 
64  JOINT_STATE_BROADCASTER_PUBLIC
66 
67  JOINT_STATE_BROADCASTER_PUBLIC
69 
70  JOINT_STATE_BROADCASTER_PUBLIC
71  controller_interface::return_type update(
72  const rclcpp::Time & time, const rclcpp::Duration & period) override;
73 
74  JOINT_STATE_BROADCASTER_PUBLIC
75  controller_interface::CallbackReturn on_init() override;
76 
77  JOINT_STATE_BROADCASTER_PUBLIC
78  controller_interface::CallbackReturn on_configure(
79  const rclcpp_lifecycle::State & previous_state) override;
80 
81  JOINT_STATE_BROADCASTER_PUBLIC
82  controller_interface::CallbackReturn on_activate(
83  const rclcpp_lifecycle::State & previous_state) override;
84 
85  JOINT_STATE_BROADCASTER_PUBLIC
86  controller_interface::CallbackReturn on_deactivate(
87  const rclcpp_lifecycle::State & previous_state) override;
88 
89 protected:
90  bool init_joint_data();
91  void init_joint_state_msg();
92  void init_dynamic_joint_state_msg();
93  bool use_all_available_interfaces() const;
94 
95 protected:
96  // Optional parameters
97  std::shared_ptr<ParamListener> param_listener_;
98  Params params_;
99  std::unordered_map<std::string, std::string> map_interface_to_joint_state_;
100 
101  // For the JointState message,
102  // we store the name of joints with compatible interfaces
103  std::vector<std::string> joint_names_;
104  std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> joint_state_publisher_;
105  std::shared_ptr<realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>>
106  realtime_joint_state_publisher_;
107 
108  // For the DynamicJointState format, we use a map to buffer values in for easier lookup
109  // This allows to preserve whatever order or names/interfaces were initialized.
110  std::unordered_map<std::string, std::unordered_map<std::string, double>> name_if_value_mapping_;
111  std::shared_ptr<rclcpp::Publisher<control_msgs::msg::DynamicJointState>>
112  dynamic_joint_state_publisher_;
113  std::shared_ptr<realtime_tools::RealtimePublisher<control_msgs::msg::DynamicJointState>>
114  realtime_dynamic_joint_state_publisher_;
115 
116  urdf::Model model_;
117  bool is_model_loaded_ = false;
118 };
119 
120 } // namespace joint_state_broadcaster
121 
122 #endif // JOINT_STATE_BROADCASTER__JOINT_STATE_BROADCASTER_HPP_
Definition: controller_interface.hpp:28
Joint State Broadcaster for all or some state in a ros2_control system.
Definition: joint_state_broadcaster.hpp:59
JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration state_interface_configuration() const override
Get configuration for controller's required state interfaces.
Definition: joint_state_broadcaster.cpp:67
JOINT_STATE_BROADCASTER_PUBLIC controller_interface::return_type update(const rclcpp::Time &time, const rclcpp::Duration &period) override
Definition: joint_state_broadcaster.cpp:351
JOINT_STATE_BROADCASTER_PUBLIC controller_interface::CallbackReturn on_init() override
Extending interface with initialization method which is individual for each controller.
Definition: joint_state_broadcaster.cpp:44
JOINT_STATE_BROADCASTER_PUBLIC controller_interface::InterfaceConfiguration command_interface_configuration() const override
Get configuration for controller's required command interfaces.
Definition: joint_state_broadcaster.cpp:61
void init_joint_state_msg()
Definition: joint_state_broadcaster.cpp:295
Configuring what command/state interfaces to claim.
Definition: controller_interface_base.hpp:58