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: