Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Get robot description from topic in GetUrdfService #2681

Merged
merged 7 commits into from
Feb 13, 2024
Merged

Get robot description from topic in GetUrdfService #2681

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
a06720a
Get robot description from topic
Abishalini Feb 12, 2024
be3f741
Update moveit_ros/move_group/src/default_capabilities/get_group_urdf_…
Abishalini Feb 12, 2024
2a726d4
Move subscriber initialization to initialize() function
Abishalini Feb 12, 2024
381f71f
Fix QoS settings
Abishalini Feb 12, 2024
1d441c7
Update error message
Abishalini Feb 12, 2024
0e9179b
Get URDF string from RDF Loader instead
Abishalini Feb 13, 2024
7db923b
Remove unused code
Abishalini Feb 13, 2024
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
25 changes: 16 additions & 9 deletions moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,10 @@ GetUrdfService::GetUrdfService() : MoveGroupCapability("get_group_urdf")

void GetUrdfService::initialize()
{
robot_description_subscriber_ = context_->moveit_cpp_->getNode()->create_subscription<std_msgs::msg::String>(
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we make sure the topic robot_description is available and wait for a given amount of time if not?

Copy link
Contributor Author

@Abishalini Abishalini Feb 12, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I tried using this rclcpp function to wait for a certain time until we get a message in the robot_description topic. But this causes a timeout error when using get_urdf service. Not sure I'm using it wrong.

I could add a while loop with a timer to check if the full_urdf_string_ variable is set.

Do you have a better solution to this?

Copy link
Member

@tylerjw tylerjw Feb 12, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I thought about this a bit more and because you have a moveit_cpp you should be able to get the URDF string like this and not need to manually handle the subscription.

If you add a function to RDFLoader to get the urdf_string_ value it contains. Maybe name it getUrdfString() -> std::string. Then you can do this:

std::string urdf_string = context_
  ->moveit_cpp_
  ->getPlanningSceneMonitor()
  ->getRobotModelLoader()
  ->getRDFLoader()
  ->getUrdfString();

I know it is ugly, but if you have a moveit_cpp and it is initialized, you should be able to get the string this way, too.

Then you could just add this call to top of the callback for your service and you shouldn't have to subscribe to the topic or read the parameter.

"robot_description", rclcpp::SystemDefaultsQoS(),
[this](const std_msgs::msg::String::ConstSharedPtr& msg) { return robotDescriptionSubscriberCallback(msg); });

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,
Expand All @@ -78,12 +82,9 @@ void GetUrdfService::initialize()
res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
return;
}
// Get robot description string
std::string full_urdf_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())
// Check if robot description string is empty
if (full_urdf_string_.empty())
{
const auto error_string =
std::string("Couldn't load the urdf from parameter server. Is the '/robot_description' parameter "
Expand All @@ -107,8 +108,8 @@ void GetUrdfService::initialize()

for (const auto& link_name : link_names)
{
const auto start = full_urdf_string.find("<link name=\"" + link_name + "\"");
auto substring = full_urdf_string.substr(start, full_urdf_string.size() - start);
const auto start = full_urdf_string_.find("<link name=\"" + link_name + "\"");
auto substring = full_urdf_string_.substr(start, full_urdf_string_.size() - start);

// Link elements can be closed either by "/>" or "</link>" so we need to consider both cases
auto const substring_without_opening = substring.substr(1, substring.size() - 2);
Expand All @@ -133,8 +134,8 @@ void GetUrdfService::initialize()
joint_names.erase(std::unique(joint_names.begin(), joint_names.end()), joint_names.end());
for (const auto& joint_name : joint_names)
{
const auto start = full_urdf_string.find("<joint name=\"" + joint_name + "\" type");
auto substring = full_urdf_string.substr(start, full_urdf_string.size() - start);
const auto start = full_urdf_string_.find("<joint name=\"" + joint_name + "\" type");
auto substring = full_urdf_string_.substr(start, full_urdf_string_.size() - start);
res->urdf_string += substring.substr(0, substring.find(JOINT_ELEMENT_CLOSING) + JOINT_ELEMENT_CLOSING.size());
RCLCPP_ERROR(getLogger(), "%s", joint_name.c_str());

Expand Down Expand Up @@ -164,6 +165,12 @@ void GetUrdfService::initialize()
} // End of callback function
);
}

void GetUrdfService::robotDescriptionSubscriberCallback(const std_msgs::msg::String::ConstSharedPtr& msg)
{
full_urdf_string_ = msg->data;
}

} // namespace move_group

#include <pluginlib/class_list_macros.hpp>
Expand Down
6 changes: 6 additions & 0 deletions moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@
#include <moveit/move_group/move_group_capability.h>
#include <moveit_msgs/srv/get_group_urdf.hpp>

#include <std_msgs/msg/string.hpp>

namespace move_group
{
/**
Expand All @@ -62,6 +64,10 @@ class GetUrdfService : public MoveGroupCapability
void initialize() override;

private:
std::string full_urdf_string_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscriber_;
rclcpp::Service<moveit_msgs::srv::GetGroupUrdf>::SharedPtr get_urdf_service_;

void robotDescriptionSubscriberCallback(const std_msgs::msg::String::ConstSharedPtr& msg);
};
} // namespace move_group
Loading