Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 30 additions & 0 deletions moveit_core/utils/src/robot_model_test_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@

/* Author: Bryce Willey */

#include <rclcpp/version.h>

#include <ament_index_cpp/get_package_share_directory.hpp>
#include <boost/algorithm/string_regex.hpp>
#include <filesystem>
Expand Down Expand Up @@ -64,10 +66,20 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& package_nam
const std::string& urdf_relative_path,
const std::string& srdf_relative_path)
{
// For Rolling, L-turtle, and newer
#if RCLCPP_VERSION_GTE(30, 0, 0)
std::filesystem::path urdf_path;
ament_index_cpp::get_package_share_directory(package_name, urdf_path);
urdf_path /= urdf_relative_path;
std::filesystem::path srdf_path;
ament_index_cpp::get_package_share_directory(package_name, srdf_path);
srdf_path /= srdf_relative_path;
#else
const auto urdf_path =
std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)) / urdf_relative_path;
const auto srdf_path =
std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name)) / srdf_relative_path;
#endif

urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path.string());
if (urdf_model == nullptr)
Expand Down Expand Up @@ -95,7 +107,13 @@ moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
{
const std::string package_name = "moveit_resources_" + robot_name + "_description";
// For Rolling, L-turtle, and newer
#if RCLCPP_VERSION_GTE(30, 0, 0)
std::filesystem::path res_path;
ament_index_cpp::get_package_share_directory(package_name, res_path);
#else
std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
#endif
std::string urdf_path;
if (robot_name == "pr2")
{
Expand Down Expand Up @@ -123,13 +141,25 @@ srdf::ModelSharedPtr loadSRDFModel(const std::string& robot_name)
if (robot_name == "pr2")
{
const std::string package_name = "moveit_resources_" + robot_name + "_description";
// For Rolling, L-turtle, and newer
#if RCLCPP_VERSION_GTE(30, 0, 0)
std::filesystem::path res_path;
ament_index_cpp::get_package_share_directory(package_name, res_path);
#else
std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
#endif
srdf_path = (res_path / "srdf/robot.xml").string();
}
else
{
const std::string package_name = "moveit_resources_" + robot_name + "_moveit_config";
// For Rolling, L-turtle, and newer
#if RCLCPP_VERSION_GTE(30, 0, 0)
std::filesystem::path res_path;
ament_index_cpp::get_package_share_directory(package_name, res_path);
#else
std::filesystem::path res_path(ament_index_cpp::get_package_share_directory(package_name));
#endif
srdf_path = (res_path / "config" / (robot_name + ".srdf")).string();
}
srdf_model->initFile(*urdf_model, srdf_path);
Expand Down
12 changes: 9 additions & 3 deletions moveit_ros/planning/rdf_loader/src/rdf_loader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@

/* Author: Ioan Sucan, Mathias Lüdtke, Dave Coleman */

#include <rclcpp/version.h>

// MoveIt
#include <moveit/rdf_loader/rdf_loader.hpp>
#include <std_msgs/msg/string.hpp>
Expand Down Expand Up @@ -217,18 +219,22 @@ bool RDFLoader::loadXmlFileToString(std::string& buffer, const std::string& path
bool RDFLoader::loadPkgFileToString(std::string& buffer, const std::string& package_name,
const std::string& relative_path, const std::vector<std::string>& xacro_args)
{
std::string package_path;
std::filesystem::path path;
try
{
package_path = ament_index_cpp::get_package_share_directory(package_name);
// For Rolling, L-turtle, and newer
#if RCLCPP_VERSION_GTE(30, 0, 0)
ament_index_cpp::get_package_share_directory(package_name, path);
#else
path = ament_index_cpp::get_package_share_directory(package_name);
#endif
}
catch (const ament_index_cpp::PackageNotFoundError& e)
{
RCLCPP_ERROR(getLogger(), "ament_index_cpp: %s", e.what());
return false;
}

std::filesystem::path path(package_path);
path = path / relative_path;

return loadXmlFileToString(buffer, path.string(), xacro_args);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@

#pragma once

#include <rclcpp/version.h>

#include <ament_index_cpp/get_package_prefix.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <filesystem>
Expand All @@ -52,7 +54,14 @@ inline std::filesystem::path getSharePath(const std::string& package_name)
{
try
{
// For Rolling, L-turtle, and newer
#if RCLCPP_VERSION_GTE(30, 0, 0)
std::filesystem::path path;
ament_index_cpp::get_package_share_directory(package_name, path);
return path;
#else
return std::filesystem::path(ament_index_cpp::get_package_share_directory(package_name));
#endif
}
catch (const std::runtime_error& e)
{
Expand Down
Loading