moveit2
The MoveIt Motion Planning Framework for ROS 2.
control_xacro_config.hpp
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 
37 #pragma once
38 
41 
42 namespace moveit_setup
43 {
44 namespace controllers
45 {
47 {
48  std::vector<std::string> command_interfaces;
49  std::vector<std::string> state_interfaces;
50 };
51 
52 inline std::vector<std::string> getAvailableInterfaceNames()
53 {
54  return { "position", "velocity", "effort" };
55 }
56 
58 {
59 public:
62  void onInit() override;
63  void loadPrevious(const std::filesystem::path& /*config_package_path*/, const YAML::Node& node) override;
64  YAML::Node saveToYaml() const override;
65  bool isConfigured() const override;
70  std::string getFilepath() const override;
71  bool hasChanges() const override;
72  std::vector<std::pair<std::string, std::string>> getArguments() const override;
73  std::vector<std::string> getCommands() const override;
81  void loadFromDescription();
82 
83  bool hasAllControlTagsInOriginal() const;
84 
89  {
90  return available_ci_;
91  }
92 
94  {
95  return default_ci_;
96  }
97 
101  void setControlInterfaces(const ControlInterfaces& ci);
102 
106  const ControlInterfaces getControlInterfaces(const std::vector<std::string>& joint_names) const;
107 
113  std::string getJointsXML() const;
114 
116  {
117  public:
118  GeneratedControlHeader(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
119  ControlXacroConfig& parent)
120  : TemplatedGeneratedFile(package_path, last_gen_time), parent_(parent)
121  {
122  }
123 
124  std::filesystem::path getRelativePath() const override
125  {
126  return std::filesystem::path("config") / (parent_.urdf_config_->getRobotName() + ".ros2_control.xacro");
127  }
128 
129  std::filesystem::path getTemplatePath() const override
130  {
131  return getSharePath("moveit_setup_controllers") / "templates" / "config" / "ros2_control.xacro";
132  }
133 
134  std::string getDescription() const override
135  {
136  return "Macro definition for required ros2_control xacro additions.";
137  }
138 
139  bool hasChanges() const override
140  {
141  return parent_.hasChanges();
142  }
143 
144  protected:
146  };
147 
149  {
150  public:
151  GeneratedInitialPositions(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
152  ControlXacroConfig& parent)
153  : YamlGeneratedFile(package_path, last_gen_time), parent_(parent)
154  {
155  }
156 
157  bool hasChanges() const override
158  {
159  return parent_.hasChanges();
160  }
161 
162  std::filesystem::path getRelativePath() const override
163  {
164  return "config/initial_positions.yaml";
165  }
166 
167  std::string getDescription() const override
168  {
169  return "Initial positions for ros2_control";
170  }
171 
172  bool writeYaml(YAML::Emitter& emitter) override;
173 
174  protected:
176  };
177 
178  void collectFiles(const std::filesystem::path& package_path, const GeneratedTime& last_gen_time,
179  std::vector<GeneratedFilePtr>& files) override
180  {
181  files.push_back(std::make_shared<GeneratedControlHeader>(package_path, last_gen_time, *this));
182  files.push_back(std::make_shared<GeneratedInitialPositions>(package_path, last_gen_time, *this));
183  }
184 
185  void collectVariables(std::vector<TemplateVariable>& variables) override;
186 
187 protected:
188  void getControlInterfaces(const std::string& joint_name, ControlInterfaces& ci) const;
189 
190  std::vector<std::string> joint_names_;
191  std::unordered_map<std::string, ControlInterfaces> original_joint_interfaces_, new_joint_interfaces_;
192 
194  srdf::Model::GroupState initial_group_state_;
195 
196  bool changed_;
197 };
198 } // namespace controllers
199 } // namespace moveit_setup
A virtual class that represents a xacro header that should be included in the modified urdf configura...
std::shared_ptr< URDFConfig > urdf_config_
Specialization of GeneratedFile for generating a text file from a template.
Definition: templates.hpp:60
bool hasChanges() const override
Returns true if this file will have changes when it is written to file.
std::string getDescription() const override
Returns an English description of this file's purpose.
std::filesystem::path getRelativePath() const override
Returns the path relative to the configuration package root.
std::filesystem::path getTemplatePath() const override
Returns the full path to the template file.
GeneratedControlHeader(const std::filesystem::path &package_path, const GeneratedTime &last_gen_time, ControlXacroConfig &parent)
GeneratedInitialPositions(const std::filesystem::path &package_path, const GeneratedTime &last_gen_time, ControlXacroConfig &parent)
bool hasChanges() const override
Returns true if this file will have changes when it is written to file.
std::filesystem::path getRelativePath() const override
Returns the path relative to the configuration package root.
std::string getDescription() const override
Returns an English description of this file's purpose.
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.
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.
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_
const ControlInterfaces & getDefaultControlInterfaces() const
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.
const ControlInterfaces & getAvailableControlInterfaces() const
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.
std::vector< std::string > getAvailableInterfaceNames()
std::filesystem::path getSharePath(const std::string &package_name)
Return a path for the given package's share folder.
Definition: utilities.hpp:50
std::filesystem::file_time_type GeneratedTime