Add documentation and format

moveit · Jan 12, 2024 · a5d92a1 · a5d92a1
1 parent 83ccf52
commit a5d92a1
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 13 deletions.
diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml
@@ -74,7 +74,7 @@
 
   <class name="move_group/GetUrdfService" type="move_group::GetUrdfService" base_class_type="move_group::MoveGroupCapability">
     <description>
-      TODO
+      Provide a capability which creates an URDF string with joints and links of a requested joint model group
     </description>
   </class>
 

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
@@ -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 <moveit/moveit_cpp/moveit_cpp.h>
-#include <moveit/utils/robot_model_test_utils.h>
 #include <moveit/move_group/capability_names.h>
 #include <moveit/utils/logger.hpp>
 #include <tinyxml2.h>
@@ -53,6 +52,7 @@ rclcpp::Logger getLogger()
 }
 const auto JOINT_ELEMENT_CLOSING = std::string("</joint>");
 const auto LINK_ELEMENT_CLOSING = std::string("</link>");
+const auto ROBOT_ELEMENT_CLOSING = std::string("</robot>")
 }  // 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<moveit_msgs::srv::GetGroupUrdf>(
-      GET_URDF_SERVICE_NAME, [this](const std::shared_ptr<moveit_msgs::srv::GetGroupUrdf::Request>& req,
-                                    const std::shared_ptr<moveit_msgs::srv::GetGroupUrdf::Response>& 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<moveit_msgs::srv::GetGroupUrdf::Request>& req,
+             const std::shared_ptr<moveit_msgs::srv::GetGroupUrdf::Response>& 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,18 +88,19 @@ void GetUrdfService::initialize()
         }
 
         // Create subgroup urdf
+        // Create header
         res->urdf_string = std::string("<?xml version=\"1.0\" ?>\n<robot name=\"") + req->group_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)
         {
           auto const start = full_urdf_string.find("<link name=\"" + link_name);
           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 += "</robot>";
+        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
@@ -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: