moveit2
The MoveIt Motion Planning Framework for ROS 2.
rdf_loader.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan, Mathias L├╝dtke, Dave Coleman */
36 
37 // MoveIt
39 #include <std_msgs/msg/string.hpp>
40 #include <ament_index_cpp/get_package_prefix.hpp>
41 #include <ament_index_cpp/get_package_share_directory.hpp>
42 #include <moveit/utils/logger.hpp>
43 
44 #include <rclcpp/duration.hpp>
45 #include <rclcpp/logger.hpp>
46 #include <rclcpp/logging.hpp>
47 #include <rclcpp/node.hpp>
48 #include <rclcpp/time.hpp>
49 
50 // C++
51 #include <fstream>
52 #include <streambuf>
53 #include <algorithm>
54 #include <chrono>
55 #include <filesystem>
56 
57 namespace rdf_loader
58 {
59 namespace
60 {
61 rclcpp::Logger getLogger()
62 {
63  return moveit::getLogger("RDFLoader");
64 }
65 } // namespace
66 
67 RDFLoader::RDFLoader(const std::shared_ptr<rclcpp::Node>& node, const std::string& ros_name,
68  bool default_continuous_value, double default_timeout)
69  : ros_name_(ros_name)
70 {
71  auto start = node->now();
72 
73  urdf_string_ = urdf_ssp_.loadInitialValue(
74  node, ros_name, [this](const std::string& new_urdf_string) { return urdfUpdateCallback(new_urdf_string); },
75  default_continuous_value, default_timeout);
76 
77  const std::string srdf_name = ros_name + "_semantic";
78  srdf_string_ = srdf_ssp_.loadInitialValue(
79  node, srdf_name, [this](const std::string& new_srdf_string) { return srdfUpdateCallback(new_srdf_string); },
80  default_continuous_value, default_timeout);
81 
82  if (!loadFromStrings())
83  {
84  return;
85  }
86 
87  RCLCPP_INFO_STREAM(getLogger(), "Loaded robot model in " << (node->now() - start).seconds() << " seconds");
88 }
89 
90 RDFLoader::RDFLoader(const std::string& urdf_string, const std::string& srdf_string)
91  : urdf_string_(urdf_string), srdf_string_(srdf_string)
92 {
93  if (!loadFromStrings())
94  {
95  return;
96  }
97 }
98 
99 bool RDFLoader::loadFromStrings()
100 {
101  std::unique_ptr<urdf::Model> urdf = std::make_unique<urdf::Model>();
102  if (!urdf->initString(urdf_string_))
103  {
104  RCLCPP_INFO(getLogger(), "Unable to parse URDF");
105  return false;
106  }
107 
108  srdf::ModelSharedPtr srdf = std::make_shared<srdf::Model>();
109  if (!srdf->initString(*urdf, srdf_string_))
110  {
111  RCLCPP_ERROR(getLogger(), "Unable to parse SRDF");
112  return false;
113  }
114 
115  urdf_ = std::move(urdf);
116  srdf_ = std::move(srdf);
117  return true;
118 }
119 
120 bool RDFLoader::isXacroFile(const std::string& path)
121 {
122  std::string lower_path = path;
123  std::transform(lower_path.begin(), lower_path.end(), lower_path.begin(), ::tolower);
124 
125  return lower_path.find(".xacro") != std::string::npos;
126 }
127 
128 bool RDFLoader::loadFileToString(std::string& buffer, const std::string& path)
129 {
130  if (path.empty())
131  {
132  RCLCPP_ERROR(getLogger(), "Path is empty");
133  return false;
134  }
135 
136  if (!std::filesystem::exists(path))
137  {
138  RCLCPP_ERROR(getLogger(), "File does not exist");
139  return false;
140  }
141 
142  std::ifstream stream(path.c_str());
143  if (!stream.good())
144  {
145  RCLCPP_ERROR(getLogger(), "Unable to load path");
146  return false;
147  }
148 
149  // Load the file to a string using an efficient memory allocation technique
150  stream.seekg(0, std::ios::end);
151  buffer.reserve(stream.tellg());
152  stream.seekg(0, std::ios::beg);
153  buffer.assign((std::istreambuf_iterator<char>(stream)), std::istreambuf_iterator<char>());
154  stream.close();
155 
156  return true;
157 }
158 
159 bool RDFLoader::loadXacroFileToString(std::string& buffer, const std::string& path,
160  const std::vector<std::string>& xacro_args)
161 {
162  buffer.clear();
163  if (path.empty())
164  {
165  RCLCPP_ERROR(getLogger(), "Path is empty");
166  return false;
167  }
168 
169  if (!std::filesystem::exists(path))
170  {
171  RCLCPP_ERROR(getLogger(), "File does not exist");
172  return false;
173  }
174 
175  std::string cmd = "ros2 run xacro xacro ";
176  for (const std::string& xacro_arg : xacro_args)
177  cmd += xacro_arg + " ";
178  cmd += path;
179 
180 #ifdef _WIN32
181  FILE* pipe = _popen(cmd.c_str(), "r");
182 #else
183  FILE* pipe = popen(cmd.c_str(), "r");
184 #endif
185  if (!pipe)
186  {
187  RCLCPP_ERROR(getLogger(), "Unable to load path");
188  return false;
189  }
190 
191  char pipe_buffer[128];
192  while (!feof(pipe))
193  {
194  if (fgets(pipe_buffer, 128, pipe) != nullptr)
195  buffer += pipe_buffer;
196  }
197 #ifdef _WIN32
198  _pclose(pipe);
199 #else
200  pclose(pipe);
201 #endif
202 
203  return true;
204 }
205 
206 bool RDFLoader::loadXmlFileToString(std::string& buffer, const std::string& path,
207  const std::vector<std::string>& xacro_args)
208 {
209  if (isXacroFile(path))
210  {
211  return loadXacroFileToString(buffer, path, xacro_args);
212  }
213 
214  return loadFileToString(buffer, path);
215 }
216 
217 bool RDFLoader::loadPkgFileToString(std::string& buffer, const std::string& package_name,
218  const std::string& relative_path, const std::vector<std::string>& xacro_args)
219 {
220  std::string package_path;
221  try
222  {
224  }
225  catch (const ament_index_cpp::PackageNotFoundError& e)
226  {
227  RCLCPP_ERROR(getLogger(), "ament_index_cpp: %s", e.what());
228  return false;
229  }
230 
231  std::filesystem::path path(package_path);
232  path = path / relative_path;
233 
234  return loadXmlFileToString(buffer, path.string(), xacro_args);
235 }
236 
237 void RDFLoader::urdfUpdateCallback(const std::string& new_urdf_string)
238 {
239  urdf_string_ = new_urdf_string;
240  if (!loadFromStrings())
241  {
242  return;
243  }
244  if (new_model_cb_)
245  {
246  new_model_cb_();
247  }
248 }
249 
250 void RDFLoader::srdfUpdateCallback(const std::string& new_srdf_string)
251 {
252  srdf_string_ = new_srdf_string;
253  if (!loadFromStrings())
254  {
255  return;
256  }
257  if (new_model_cb_)
258  {
259  new_model_cb_();
260  }
261 }
262 } // namespace rdf_loader
static bool loadXmlFileToString(std::string &buffer, const std::string &path, const std::vector< std::string > &xacro_args)
helper that branches between loadFileToString() and loadXacroFileToString() based on result of isXacr...
Definition: rdf_loader.cpp:206
static bool isXacroFile(const std::string &path)
determine if given path points to a xacro file
Definition: rdf_loader.cpp:120
static bool loadPkgFileToString(std::string &buffer, const std::string &package_name, const std::string &relative_path, const std::vector< std::string > &xacro_args)
helper that generates a file path based on package name and relative file path to package
Definition: rdf_loader.cpp:217
RDFLoader(const std::shared_ptr< rclcpp::Node > &node, const std::string &ros_name="robot_description", bool default_continuous_value=false, double default_timeout=10.0)
Default constructor.
Definition: rdf_loader.cpp:67
static bool loadFileToString(std::string &buffer, const std::string &path)
load file from given path into buffer
Definition: rdf_loader.cpp:128
static bool loadXacroFileToString(std::string &buffer, const std::string &path, const std::vector< std::string > &xacro_args)
run xacro with the given args on the file, return result in buffer
Definition: rdf_loader.cpp:159
std::string loadInitialValue(const std::shared_ptr< rclcpp::Node > &node, const std::string &name, const StringCallback &parent_callback={}, bool default_continuous_value=false, double default_timeout=10.0)
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition: logger.cpp:79
string package_name
Definition: setup.py:4