diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml index a68f140621..35e379e1f2 100644 --- a/moveit_ros/move_group/default_capabilities_plugin_description.xml +++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml @@ -74,7 +74,7 @@ - TODO + Provide a capability which creates an URDF string with joints and links of a requested joint model group diff --git a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp index 543f19a45a..d2dc743121 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2023, PickNik Inc. + * Copyright (c) 2024, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,7 +37,6 @@ #include "get_group_urdf_capability.h" #include -#include #include #include #include @@ -53,6 +52,7 @@ rclcpp::Logger getLogger() } const auto JOINT_ELEMENT_CLOSING = std::string(""); const auto LINK_ELEMENT_CLOSING = std::string(""); +const auto ROBOT_ELEMENT_CLOSING = std::string("") } // namespace GetUrdfService::GetUrdfService() : MoveGroupCapability("get_group_urdf") @@ -62,10 +62,10 @@ GetUrdfService::GetUrdfService() : MoveGroupCapability("get_group_urdf") void GetUrdfService::initialize() { get_urdf_service_ = context_->moveit_cpp_->getNode()->create_service( - GET_URDF_SERVICE_NAME, [this](const std::shared_ptr& req, - const std::shared_ptr& res) { - auto const robot_model = context_->moveit_cpp_->getRobotModel(); - auto const subgroup = robot_model->getJointModelGroup(req->group_name); + GET_URDF_SERVICE_NAME, + [this](const std::shared_ptr& req, + const std::shared_ptr& res) { + auto const subgroup = context_->moveit_cpp_->getRobotModel()->getJointModelGroup(req->group_name); // Check if group exists in loaded robot model if (!subgroup) { @@ -78,6 +78,7 @@ void GetUrdfService::initialize() auto full_urdf_string = std::string(""); context_->moveit_cpp_->getNode()->get_parameter_or("robot_description", full_urdf_string, std::string("")); + // Check if string is empty if (full_urdf_string.empty()) { RCLCPP_ERROR(getLogger(), "Couldn't load the urdf from parameter server. Is the /robot_description parameter " @@ -87,10 +88,11 @@ void GetUrdfService::initialize() } // Create subgroup urdf + // Create header res->urdf_string = std::string("\ngroup_name + std::string("\" xmlns:xacro=\"http://ros.org/wiki/xacro\">"); - // Create link list + // Create links auto const link_names = subgroup->getLinkModelNames(); for (const auto& link_name : link_names) { @@ -98,7 +100,7 @@ void GetUrdfService::initialize() auto substring = full_urdf_string.substr(start, full_urdf_string.size() - start); res->urdf_string += substring.substr(0, substring.find(LINK_ELEMENT_CLOSING) + LINK_ELEMENT_CLOSING.size()); } - // Create joint list + // Create joints auto const joint_names = subgroup->getJointModelNames(); for (const auto& joint_name : joint_names) { @@ -107,18 +109,19 @@ void GetUrdfService::initialize() res->urdf_string += substring.substr(0, substring.find(JOINT_ELEMENT_CLOSING) + JOINT_ELEMENT_CLOSING.size()); } // Create closing - res->urdf_string += ""; + res->urdf_string += ROBOT_ELEMENT_CLOSING; // Validate xml file tinyxml2::XMLDocument group_urdf_xml; group_urdf_xml.Parse(full_urdf_string.c_str()); if (group_urdf_xml.Error()) { - RCLCPP_ERROR(getLogger(), "Failed to parse urdf. tinyxml returned '%s'", group_urdf_xml.ErrorStr()); + RCLCPP_ERROR(getLogger(), "Failed to create valid urdf. tinyxml returned '%s'", group_urdf_xml.ErrorStr()); res->success = false; return; } - }); + } // End of callback function + ); } } // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h index f3288a7913..05fb6b10ff 100644 --- a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h @@ -33,7 +33,7 @@ *********************************************************************/ /* Author: Sebastian Jahr - Desc: TODO */ + Desc: This capability creates an URDF string with joints and links of a requested joint model group */ #pragma once @@ -42,11 +42,23 @@ namespace move_group { +/** + * @brief Move group capability to create an URDF string for a joint model group + * + */ class GetUrdfService : public MoveGroupCapability { public: + /** + * @brief Constructor + * + */ GetUrdfService(); + /** + * @brief Initializes service when plugin is loaded + * + */ void initialize() override; private: