Skip to content

Use a separate node for the srv IK client #3704

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

Open
stephanie-eng wants to merge 2 commits into
base: main
Choose a base branch
from
+13 −5
Open

Use a separate node for the srv IK client #3704

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
c339fba
Use a separate node for the srv IK client
stephanie-eng Mar 13, 2026
1285c4f
Merge branch 'main' into seng/srv-ik-node2
stephanie-eng Mar 25, 2026
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
4 changes: 4 additions & 0 deletions ...tics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#pragma once

// ROS2
#include <rclcpp/executor.hpp>
#include <rclcpp/rclcpp.hpp>

// System
Expand Down Expand Up @@ -150,6 +151,9 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase
int num_possible_redundant_joints_;

rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr ik_service_client_;
rclcpp::Node::SharedPtr ik_node_;
rclcpp::Executor::SharedPtr ik_executor_;
std::thread ik_thread_;

rclcpp::Node::SharedPtr node_;

Expand Down
14 changes: 9 additions & 5 deletions moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,9 +124,14 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const
robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
robot_state_->setToDefaultValues();

ik_node_ = std::make_shared<rclcpp::Node>("srv_ik_client", node_->get_namespace());

// Create the ROS2 service client
RCLCPP_DEBUG(getLogger(), "IK Service client topic : %s", params_.kinematics_solver_service_name.c_str());
ik_service_client_ = node_->create_client<moveit_msgs::srv::GetPositionIK>(params_.kinematics_solver_service_name);
ik_service_client_ = ik_node_->create_client<moveit_msgs::srv::GetPositionIK>(params_.kinematics_solver_service_name);
ik_executor_ = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
ik_executor_->add_node(ik_node_);
ik_thread_ = std::thread([this]() { ik_executor_->spin(); });

if (!ik_service_client_->wait_for_service(std::chrono::seconds(1)))
{ // wait 0.1 seconds, blocking
Expand Down Expand Up @@ -312,8 +317,8 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::msg:

RCLCPP_DEBUG(getLogger(), "Calling service: %s", ik_service_client_->get_service_name());
auto result_future = ik_service_client_->async_send_request(ik_srv);
const auto& response = result_future.get();
if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS)
const auto response = result_future.get();
if (response)
{
// Check error code
error_code.val = response->error_code.val;
Expand Down Expand Up @@ -352,7 +357,6 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::msg:
error_code.val = error_code.FAILURE;
return false;
}

// Get just the joints we are concerned about in our planning group
robot_state_->copyJointGroupPositions(joint_model_group_, solution);

Expand Down Expand Up @@ -382,7 +386,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::msg:
}
}

RCLCPP_INFO(getLogger(), "IK Solver Succeeded!");
RCLCPP_DEBUG(getLogger(), "IK Solver Succeeded!");
return true;
}

Expand Down
Loading