moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
moveit_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
39
40namespace moveit_setup
41{
42namespace controllers
43{
44void MoveItControllersConfig::loadPrevious(const std::filesystem::path& package_path, const YAML::Node& /*node*/)
45{
46 changed_ = false;
47 controllers_.clear();
49
50 // Load moveit controllers yaml file if available-----------------------------------------------
51 std::filesystem::path ros_controllers_yaml_path = package_path / MOVEIT_CONTROLLERS_YAML;
52 std::ifstream input_stream(ros_controllers_yaml_path);
53 if (!input_stream.good())
54 {
55 RCLCPP_WARN_STREAM(*logger_, "Does not exist " << ros_controllers_yaml_path);
56 return;
57 }
58
59 // Begin parsing
60 try
61 {
62 // Used in parsing controllers
63 ControllerInfo control_setting;
64 YAML::Node doc = YAML::Load(input_stream);
65 YAML::Node controllers = doc["moveit_simple_controller_manager"];
66
67 std::vector<std::string> controller_names;
68 getYamlProperty(controllers, "controller_names", controller_names);
69
70 // Loop through all controllers
71 for (const std::string& controller_name : controller_names)
72 {
73 const YAML::Node& cnode = controllers[controller_name];
74 if (!cnode.IsDefined())
75 {
76 RCLCPP_WARN_STREAM(*logger_, "Configuration for controller " << controller_name << " does not exist! Ignoring.");
77 continue;
78 }
79
80 if (!parseController(controller_name, cnode))
81 {
82 return;
83 }
84 }
85
86 YAML::Node trajectory_execution = doc["trajectory_execution"];
87 if (trajectory_execution.IsDefined() && trajectory_execution.IsMap())
88 {
89 for (const auto& kv : trajectory_execution)
90 {
91 trajectory_parameters_[kv.first.as<std::string>()] = kv.second;
92 }
93 }
94 }
95 catch (YAML::ParserException& e) // Catch errors
96 {
97 RCLCPP_ERROR_STREAM(*logger_, e.what());
98 }
99}
100
101// ******************************************************************************************
102// Helper function for parsing an individual Controller from moveit_controllers yaml file
103// ******************************************************************************************
104bool MoveItControllersConfig::parseController(const std::string& name, const YAML::Node& controller_node)
105{
106 // Used in parsing controllers
107 ControllerInfo control_setting;
108 control_setting.name_ = name;
109 getYamlProperty(controller_node, "type", control_setting.type_);
110 if (control_setting.type_.empty())
111 {
112 RCLCPP_ERROR_STREAM(*logger_, "Couldn't parse type for controller " << name << " in moveit_controllers.yaml");
113 return false;
114 }
115
116 for (const std::string parameter : { "action_ns", "default" })
117 {
118 if (controller_node[parameter].IsDefined())
119 {
120 control_setting.parameters_[parameter] = controller_node[parameter].as<std::string>();
121 }
122 }
123
124 const YAML::Node& joints_node = controller_node["joints"];
125
126 if (joints_node.IsSequence())
127 {
128 control_setting.joints_ = joints_node.as<std::vector<std::string>>();
129 }
130 else if (joints_node.IsDefined())
131 {
132 control_setting.joints_.push_back(joints_node.as<std::string>());
133 }
134 if (control_setting.joints_.empty())
135 {
136 RCLCPP_ERROR_STREAM(*logger_, "Couldn't parse joints for controller " << name << " in moveit_controllers.yaml");
137 return false;
138 }
139 // All required fields were parsed correctly
140 controllers_.push_back(control_setting);
141 return true;
142}
143
144// ******************************************************************************************
145// Output controllers config files
146// ******************************************************************************************
148{
149 emitter << YAML::Comment("MoveIt uses this configuration for controller management");
150 emitter << YAML::Newline;
151 emitter << YAML::BeginMap;
152 {
153 if (!parent_.trajectory_parameters_.empty())
154 {
155 emitter << YAML::Key << "trajectory_execution" << YAML::Value;
156 emitter << YAML::BeginMap;
157 for (const auto& kv : parent_.trajectory_parameters_)
158 {
159 emitter << YAML::Key << kv.first << YAML::Value << kv.second;
160 }
161 emitter << YAML::EndMap;
162 }
163
164 emitter << YAML::Key << "moveit_controller_manager" << YAML::Value
165 << "moveit_simple_controller_manager/MoveItSimpleControllerManager";
166 emitter << YAML::Newline;
167 emitter << YAML::Newline;
168
169 emitter << YAML::Key << "moveit_simple_controller_manager" << YAML::Value;
170 emitter << YAML::BeginMap;
171 {
172 emitter << YAML::Key << "controller_names";
173 emitter << YAML::Value;
174 emitter << YAML::BeginSeq;
175 for (const ControllerInfo& ci : parent_.controllers_)
176 {
177 emitter << ci.name_;
178 }
179 emitter << YAML::EndSeq;
180 emitter << YAML::Newline;
181 emitter << YAML::Newline;
182
183 for (const auto& controller : parent_.controllers_)
184 {
185 emitter << YAML::Key << controller.name_;
186 emitter << YAML::Value;
187 emitter << YAML::BeginMap;
188 {
189 emitter << YAML::Key << "type" << YAML::Value << controller.type_;
190
191 // Write joints
192 emitter << YAML::Key << "joints";
193 emitter << YAML::Value;
194 emitter << YAML::BeginSeq;
195
196 // Iterate through the joints
197 for (const std::string& joint : controller.joints_)
198 {
199 emitter << joint;
200 }
201 emitter << YAML::EndSeq;
202
203 for (const auto& pair : controller.parameters_)
204 {
205 emitter << YAML::Key << pair.first;
206 emitter << YAML::Value << pair.second;
207 }
208 }
209 emitter << YAML::EndMap;
210 }
211 }
212 emitter << YAML::EndMap;
213 }
214 emitter << YAML::EndMap;
215
216 return true;
217}
218
219} // namespace controllers
220} // namespace moveit_setup
221
222#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< rclcpp::Logger > logger_
Definition config.hpp:164
std::vector< ControllerInfo > controllers_
Controllers config data.
bool parseController(const std::string &name, const YAML::Node &controller)
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())
std::map< std::string, std::string > parameters_