moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
modified_urdf_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{
45
47{
48 return !getIncludedXacros().empty();
49}
50
51void ModifiedUrdfConfig::loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node)
52{
53 std::vector<std::string> xacro_names_vector;
54 if (node.IsDefined())
55 getYamlProperty(node, "xacros", xacro_names_vector);
56 cached_xacro_names_ = std::set(xacro_names_vector.begin(), xacro_names_vector.end());
57}
58
60{
61 YAML::Node node;
62 node["xacros"] = getIncludedXacroNames();
63 return node;
64}
65
67{
68 // Returns true if any of the included xacros are configured and have individually changed
69 // or the list of included xacros have changed.
70 unsigned int count = 0;
71 for (auto& pair : getIncludedXacroMap())
72 {
73 if (!pair.second->isConfigured())
74 {
75 continue;
76 }
77 if (pair.second->hasChanges() || cached_xacro_names_.count(pair.first) == 0)
78 {
79 return true;
80 }
81 count++;
82 }
83
84 return count != cached_xacro_names_.size();
85}
86
87void ModifiedUrdfConfig::collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
88 std::vector<GeneratedFilePtr>& files)
89{
90 files.push_back(std::make_shared<GeneratedModifiedURDF>(package_path, last_gen_time, *this));
91
92 auto xacro_names = getIncludedXacroNames();
93 cached_xacro_names_ = std::set<std::string>(xacro_names.begin(), xacro_names.end());
94}
95
96void ModifiedUrdfConfig::collectDependencies(std::set<std::string>& packages) const
97{
98 packages.insert("xacro");
99}
100
101void ModifiedUrdfConfig::collectVariables(std::vector<TemplateVariable>& variables)
102{
103 std::string args = "", imports = "", commands = "";
104
105 for (const auto& pair : getIncludedXacroMap())
106 {
107 const IncludedXacroConfig::Ptr& xacro = pair.second;
108 if (!xacro->isConfigured())
109 {
110 continue;
111 }
112 for (const std::pair<std::string, std::string>& argument : xacro->getArguments())
113 {
114 args += " <xacro:arg name=\"";
115 args += argument.first;
116 args += "\" default=\"";
117 args += argument.second;
118 args += "\" />\n";
119 }
120
121 imports += " <!-- Import ";
122 imports += pair.first;
123 imports += " -->\n";
124
125 imports += " <xacro:include filename=\"";
126 imports += xacro->getFilepath();
127 imports += "\" />\n\n";
128
129 for (const std::string& command : xacro->getCommands())
130 {
131 commands += " ";
132 commands += command;
133 commands += "\n";
134 }
135 }
136
137 variables.push_back(TemplateVariable("MODIFIED_XACRO_ARGS", args));
138 variables.push_back(TemplateVariable("MODIFIED_XACRO_IMPORTS", imports));
139 variables.push_back(TemplateVariable("MODIFIED_XACRO_COMMANDS", commands));
140}
141} // namespace moveit_setup
142
143#include <pluginlib/class_list_macros.hpp> // NOLINT
PLUGINLIB_EXPORT_CLASS(cached_ik_kinematics_plugin::CachedIKKinematicsPlugin< kdl_kinematics_plugin::KDLKinematicsPlugin >, kinematics::KinematicsBase)
std::shared_ptr< IncludedXacroConfig > Ptr
A configuration that stores info about modifications to the URDF.
void onInit() override
Overridable initialization method.
void collectDependencies(std::set< std::string > &packages) const override
Collect the package dependencies generated by this configuration.
void collectVariables(std::vector< TemplateVariable > &variables) override
Collect key/value pairs for use in templates.
std::vector< IncludedXacroConfig::Ptr > getIncludedXacros() const
YAML::Node saveToYaml() const override
Optionally save "meta" information for saving in the .setup_assistant yaml file.
bool isConfigured() const override
Return true if this part of the configuration is completely set up.
void loadPrevious(const std::filesystem::path &, const YAML::Node &node) override
Loads the configuration from an existing MoveIt configuration.
std::shared_ptr< URDFConfig > urdf_config_
std::unordered_map< std::string, IncludedXacroConfig::Ptr > getIncludedXacroMap() const
void collectFiles(const std::filesystem::path &package_path, const GeneratedTime &last_gen_time, std::vector< GeneratedFilePtr > &files) override
Collect the files generated by this configuration and add them to the vector.
bool hasChanges() const
Returns true if this or any of the included xacros have changes, requiring the URDF to be regenerated...
std::vector< std::string > getIncludedXacroNames() const
std::set< std::string > cached_xacro_names_
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
bool getYamlProperty(const YAML::Node &node, const std::string &key, T &storage, const T &default_value=T())
std::filesystem::file_time_type GeneratedTime
Simple Key/value pair for templates.
Definition templates.hpp:47