moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
ros2_controllers_config.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2022, Metro Robots
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Metro Robots nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: David V. Lu!! */
36
38
39namespace moveit_setup
40{
41namespace controllers
42{
47
48void ROS2ControllersConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& /*node*/)
49{
50 changed_ = false;
51 controllers_.clear();
52
53 // Load ros2 controllers yaml file if available-----------------------------------------------
54 std::filesystem::path ros_controllers_yaml_path = package_path / CONTROLLERS_YAML;
55 std::ifstream input_stream(ros_controllers_yaml_path);
56 if (!input_stream.good())
57 {
58 RCLCPP_WARN_STREAM(*logger_, "Does not exist " << ros_controllers_yaml_path);
59 return;
60 }
61
62 // Begin parsing
63 try
64 {
65 YAML::Node config = YAML::Load(input_stream);
66
67 // Parse Controller Manager Config
68 const YAML::Node& cm_node = config["controller_manager"]["ros__parameters"];
69 if (!cm_node.IsDefined())
70 {
71 return;
72 }
73 for (const auto& it : cm_node)
74 {
75 if (!it.second.IsMap())
76 continue;
77
78 const auto& type_node = it.second["type"];
79 if (!type_node.IsDefined())
80 {
81 continue;
82 }
83
84 ControllerInfo controller;
85 controller.name_ = it.first.as<std::string>();
86 controller.type_ = type_node.as<std::string>();
87
88 if (controller.type_ != "joint_state_broadcaster/JointStateBroadcaster")
89 {
90 controllers_.push_back(controller);
91 }
92 }
93
94 // ParseIndividual Controller Configs
95 for (auto& controller : controllers_)
96 {
97 const YAML::Node& controller_node = config[controller.name_]["ros__parameters"];
98 if (!controller_node.IsDefined() || !controller_node.IsMap())
99 {
100 continue;
101 }
102 auto jnode = controller_node["joints"];
103 if (jnode.IsDefined() && jnode.IsSequence())
104 {
105 // Looping through sequences with yaml is sometimes buggy when using iterators
106 // so here we use a "classic" loop and disable the clang-tidy rule
107 for (std::size_t i = 0; i < jnode.size(); i++) // NOLINT(modernize-loop-convert)
108 {
109 controller.joints_.push_back(jnode[i].as<std::string>());
110 }
111 }
112
113 if (controller.joints_.empty())
114 {
115 std::string one_joint;
116 getYamlProperty(controller_node, "joint", one_joint);
117 if (!one_joint.empty())
118 {
119 controller.joints_.push_back(one_joint);
120 }
121 }
122 }
123 }
124 catch (YAML::ParserException& e) // Catch errors
125 {
126 RCLCPP_ERROR_STREAM(*logger_, e.what());
127 }
128}
129
130const ControlInterfaces ROS2ControllersConfig::getControlInterfaces(const std::vector<std::string>& joint_names) const
131{
132 return control_xacro_config_->getControlInterfaces(joint_names);
133}
134
135// ******************************************************************************************
136// Output controllers config files
137// ******************************************************************************************
139{
140 emitter << YAML::Comment("This config file is used by ros2_control");
141 emitter << YAML::BeginMap;
142 {
143 // Output controller Manager Config
144 emitter << YAML::Key << "controller_manager";
145 emitter << YAML::Value;
146 emitter << YAML::BeginMap;
147 {
148 emitter << YAML::Key << "ros__parameters";
149 emitter << YAML::Value;
150 emitter << YAML::BeginMap;
151 {
152 emitter << YAML::Key << "update_rate" << YAML::Value << 100; // Hz
153 emitter << YAML::Comment("Hz");
154 emitter << YAML::Newline;
155 emitter << YAML::Newline;
156
157 // Output Types
158 for (const ControllerInfo& ci : parent_.controllers_)
159 {
160 emitter << YAML::Key << ci.name_;
161 emitter << YAML::Value;
162 emitter << YAML::BeginMap;
163 {
164 emitter << YAML::Key << "type" << YAML::Value << ci.type_;
165 }
166 emitter << YAML::Newline << YAML::Newline;
167 emitter << YAML::EndMap;
168 }
169
170 emitter << YAML::Key << "joint_state_broadcaster";
171 emitter << YAML::Value;
172 emitter << YAML::BeginMap;
173 {
174 emitter << YAML::Key << "type" << YAML::Value << "joint_state_broadcaster/JointStateBroadcaster";
175 }
176 emitter << YAML::EndMap;
177 }
178 emitter << YAML::EndMap;
179 }
180 emitter << YAML::EndMap;
181 emitter << YAML::Newline;
182 emitter << YAML::Newline;
183 }
184 // Output Controller Configs
185 for (const ControllerInfo& ci : parent_.controllers_)
186 {
187 emitter << YAML::Key << ci.name_;
188 emitter << YAML::Value;
189 emitter << YAML::BeginMap;
190 {
191 emitter << YAML::Key << "ros__parameters";
192 emitter << YAML::Value;
193 emitter << YAML::BeginMap;
194 {
195 if (ci.type_ != "position_controllers/GripperActionController")
196 {
197 emitter << YAML::Key << "joints" << YAML::Value << ci.joints_;
198 }
199 else
200 {
201 emitter << YAML::Key << "joint" << YAML::Value << ci.joints_[0];
202 }
203
204 if (ci.type_ == "joint_trajectory_controller/JointTrajectoryController")
205 {
206 const ControlInterfaces interfaces = parent_.getControlInterfaces(ci.joints_);
207 emitter << YAML::Key << "command_interfaces" << YAML::Value << interfaces.command_interfaces;
208 emitter << YAML::Key << "state_interfaces" << YAML::Value << interfaces.state_interfaces;
209 emitter << YAML::Key << "allow_nonzero_velocity_at_trajectory_end" << YAML::Value << true;
210 }
211 }
212 emitter << YAML::EndMap;
213 }
214 emitter << YAML::EndMap;
215 }
216 emitter << YAML::EndMap;
217
218 return true;
219}
220
221} // namespace controllers
222} // namespace moveit_setup
223
224#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
where all the data for each part of the configuration is stored.
Definition config.hpp:58
std::shared_ptr< DataWarehouse > config_data_
Definition config.hpp:161
std::shared_ptr< rclcpp::Logger > logger_
Definition config.hpp:164
std::vector< ControllerInfo > controllers_
Controllers config data.
void onInit() override
Overridable initialization method.
const ControlInterfaces getControlInterfaces(const std::vector< std::string > &joint_names) const
std::shared_ptr< ControlXacroConfig > control_xacro_config_
void loadPrevious(const std::filesystem::path &package_path, const YAML::Node &node) override
Loads the configuration from an existing MoveIt configuration.
bool getYamlProperty(const YAML::Node &node, const std::string &key, T &storage, const T &default_value=T())