moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
control_xacro_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#include <tinyxml2.h>
40#include <algorithm>
41
42namespace moveit_setup
43{
44namespace controllers
45{
54
55void ControlXacroConfig::loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node)
56{
57 if (!node.IsDefined())
58 {
59 return;
60 }
61
65 changed_ = false;
66}
67
69{
70 YAML::Node node;
71 node["command"] = default_ci_.command_interfaces;
72 node["state"] = default_ci_.state_interfaces;
73 return node;
74}
75
77{
78 return !new_joint_interfaces_.empty();
79}
80
82{
83 return urdf_config_->getRobotName() + ".ros2_control.xacro";
84}
85
87{
88 return changed_;
89}
90
91std::vector<std::pair<std::string, std::string>> ControlXacroConfig::getArguments() const
92{
93 std::vector<std::pair<std::string, std::string>> arguments;
94 arguments.push_back(std::make_pair("initial_positions_file", "initial_positions.yaml"));
95 return arguments;
96}
97
98std::vector<std::string> ControlXacroConfig::getCommands() const
99{
100 std::string command = "<xacro:";
101 command += urdf_config_->getRobotName();
102 command += "_ros2_control name=\"FakeSystem\" initial_positions_file=\"$(arg initial_positions_file)\"/>";
103 return { command };
104}
105
106void getInterfaceNames(const tinyxml2::XMLElement* joint_el, const std::string& element_name,
107 std::vector<std::string>& interface_names)
108{
109 for (const tinyxml2::XMLElement* el = joint_el->FirstChildElement(element_name.c_str()); el != nullptr;
110 el = el->NextSiblingElement())
111 {
112 interface_names.push_back(el->Attribute("name"));
113 }
114}
115
117{
118 // Reset Data
120 joint_names_.clear();
121
122 // Load the joint names for all joints used by the groups
123 auto srdf_config = config_data_->get<SRDFConfig>("srdf");
124 for (const std::string& group_name : srdf_config->getGroupNames())
125 {
126 for (const std::string& joint_name : srdf_config->getJointNames(group_name, true, false)) // exclude passive
127 {
128 if (std::find(joint_names_.begin(), joint_names_.end(), joint_name) == joint_names_.end())
129 {
130 // This is a new joint, add to list of joint names
131 joint_names_.push_back(joint_name);
132 }
133 }
134 }
135
136 // Read the URDF
137 tinyxml2::XMLDocument urdf_xml;
138 using tinyxml2::XMLElement;
139
140 auto urdf_config = config_data_->get<URDFConfig>("urdf");
141 urdf_xml.Parse(urdf_config->getURDFContents().c_str());
142 for (XMLElement* control_el = urdf_xml.FirstChildElement("ros2_control"); control_el != nullptr;
143 control_el = control_el->NextSiblingElement())
144 {
145 for (XMLElement* joint_el = control_el->FirstChildElement("joint"); joint_el != nullptr;
146 joint_el = joint_el->NextSiblingElement())
147 {
148 std::string joint_name = joint_el->Attribute("name");
149
150 // Parse the interfaces
152 getInterfaceNames(joint_el, "command_interface", ci.command_interfaces);
153 getInterfaceNames(joint_el, "state_interface", ci.state_interfaces);
154 original_joint_interfaces_[joint_name] = ci;
155 }
156 }
158}
159
164
166{
167 default_ci_ = ci;
168 for (const std::string& joint_name : joint_names_)
169 {
170 if (original_joint_interfaces_.count(joint_name))
171 {
172 continue;
173 }
174
176
177 // Setup the initial state
178 std::vector<double> joint_value;
179 // TODO: There could be an option to load a group state from the RobotPoses
180 // For now, just assume one DOF, and it is zero
181 joint_value.push_back(0.0);
182 initial_group_state_.joint_values_[joint_name] = joint_value;
183 }
184
185 changed_ = true;
186}
187
188void uniqueMerge(std::vector<std::string>& main, const std::vector<std::string>& addition)
189{
190 for (const std::string& s : addition)
191 {
192 if (std::find(main.begin(), main.end(), s) == main.end())
193 {
194 main.push_back(s);
195 }
196 }
197}
198
199bool getControlInterfaceHelper(const std::unordered_map<std::string, ControlInterfaces>& interfaces,
200 const std::string& joint_name, ControlInterfaces& ci)
201{
202 const auto& it = interfaces.find(joint_name);
203 if (it == interfaces.end())
204 {
205 return false;
206 }
207 const ControlInterfaces& found_interfaces = it->second;
209 uniqueMerge(ci.state_interfaces, found_interfaces.state_interfaces);
210 return true;
211}
212
213void ControlXacroConfig::getControlInterfaces(const std::string& joint_name, ControlInterfaces& ci) const
214{
216 {
217 return;
218 }
220}
221
222const ControlInterfaces ControlXacroConfig::getControlInterfaces(const std::vector<std::string>& joint_names) const
223{
225 for (const std::string& joint_name : joint_names)
226 {
227 getControlInterfaces(joint_name, ci);
228 }
229 return ci;
230}
231
233{
234 std::string joints = "";
235 const std::string tab = " ";
236 // Loop through all joints to preserve joint ordering
237 for (const std::string& joint_name : joint_names_)
238 {
239 auto pair = new_joint_interfaces_.find(joint_name);
240 if (pair == new_joint_interfaces_.end())
241 {
242 continue;
243 }
244
245 const ControlInterfaces& ci = pair->second;
246
247 joints += tab;
248 joints += "<joint name=\"" + joint_name + "\">\n";
249 for (const std::string& command_interface : ci.command_interfaces)
250 {
251 joints += tab;
252 joints += " <command_interface name=\"";
253 joints += command_interface;
254 joints += "\"/>\n";
255 }
256 for (const std::string& state_interface : ci.state_interfaces)
257 {
258 joints += tab;
259 joints += " <state_interface name=\"";
260 joints += state_interface;
261 if (state_interface == "position")
262 {
263 joints += "\">\n";
264 joints += tab;
265 joints += " <param name=\"initial_value\">${initial_positions['";
266 joints += joint_name;
267 joints += "']}</param>\n";
268 joints += tab;
269 joints += " </state_interface>\n";
270 }
271 else
272 {
273 joints += "\"/>\n";
274 }
275 }
276 joints += tab;
277 joints += "</joint>\n";
278 }
279 return joints;
280}
282{
283 emitter << YAML::Comment("Default initial positions for " + parent_.urdf_config_->getRobotName() +
284 "'s ros2_control fake system");
285 emitter << YAML::Newline;
286 emitter << YAML::BeginMap;
287 {
288 emitter << YAML::Key << "initial_positions";
289 emitter << YAML::Value;
290 emitter << YAML::BeginMap;
291 for (const auto& pair : parent_.initial_group_state_.joint_values_)
292 {
293 emitter << YAML::Key << pair.first;
294
295 const std::vector<double>& jv = pair.second;
296
297 emitter << YAML::Value;
298 if (jv.size() == 1)
299 {
300 emitter << jv[0];
301 }
302 else
303 {
304 emitter << jv;
305 }
306 }
307 emitter << YAML::EndMap;
308 }
309 emitter << YAML::EndMap;
310
311 return true;
312}
313
314void ControlXacroConfig::collectVariables(std::vector<TemplateVariable>& variables)
315{
316 variables.push_back(TemplateVariable("ROS2_CONTROL_JOINTS", getJointsXML()));
317}
318
319} // namespace controllers
320} // namespace moveit_setup
321
322#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
std::shared_ptr< URDFConfig > urdf_config_
void onInit() override
Overridable initialization method.
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::unordered_map< std::string, ControlInterfaces > original_joint_interfaces_
A list of all joints used by the current SRDF groups.
void loadPrevious(const std::filesystem::path &, const YAML::Node &node) override
Loads the configuration from an existing MoveIt configuration.
std::string getJointsXML() const
Return the additional joint xml needed for ros2_control tags.
YAML::Node saveToYaml() const override
Optionally save "meta" information for saving in the .setup_assistant yaml file.
void onInit() override
Overridable initialization method.
std::unordered_map< std::string, ControlInterfaces > new_joint_interfaces_
std::string getFilepath() const override
The file path to use in the <xacro:include> tag.
bool hasChanges() const override
Returns if the xacro and its properties have changed, resulting in the whole urdf needing regeneratio...
std::vector< std::pair< std::string, std::string > > getArguments() const override
Returns a list of name/value pairs for arguments that the modified urdf should have.
std::vector< std::string > getCommands() const override
Return a list of additional commands that need to be inserted after the xacro is included.
const ControlInterfaces getControlInterfaces(const std::vector< std::string > &joint_names) const
Get all the control interfaces for all of the specified joint names.
void loadFromDescription()
Load the original command interfaces from the original (unmodified) URDF.
void setControlInterfaces(const ControlInterfaces &ci)
Use the specified controller interfaces for all the lacking joints.
bool isConfigured() const override
Return true if this part of the configuration is completely set up.
void collectVariables(std::vector< TemplateVariable > &variables) override
Collect key/value pairs for use in templates.
bool getControlInterfaceHelper(const std::unordered_map< std::string, ControlInterfaces > &interfaces, const std::string &joint_name, ControlInterfaces &ci)
void uniqueMerge(std::vector< std::string > &main, const std::vector< std::string > &addition)
std::vector< std::string > getAvailableInterfaceNames()
void getInterfaceNames(const tinyxml2::XMLElement *joint_el, const std::string &element_name, std::vector< std::string > &interface_names)
bool getYamlProperty(const YAML::Node &node, const std::string &key, T &storage, const T &default_value=T())
Simple Key/value pair for templates.
Definition templates.hpp:47
int main(int argc, char **argv)