robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

refactor(mtp_jtc): better joint_position waiting

Browse source
This commit is contained in:
Ilya Uraev 2025-03-10 13:47:28 +03:00
parent 3eda33fc8b
commit 276f59f434
1 changed files with 28 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

35
rbs_skill_servers/src/mtp_jtc.cpp
View file

@ -2,6 +2,8 @@
#include "control_msgs/action/follow_joint_trajectory.hpp"
#include "kdl/chainiksolverpos_lma.hpp"
#include "rbs_skill_servers/base_skill.hpp"
#include <chrono>
#include <condition_variable>
#include <cstddef>
#include <iterator>
#include <kdl/chain.hpp>
@ -13,6 +15,7 @@
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <memory>
#include <mutex>
#include <rbs_skill_interfaces/action/detail/moveit_send_pose__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/parameter_client.hpp>
@ -81,17 +84,28 @@ protected:
trajectory_goal.trajectory.joint_names = m_joint_names;
const int max_wait_iterations = 100;
int wait_count = 0;
while (m_current_joint_positions.empty() &&
wait_count++ < max_wait_iterations) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (m_current_joint_positions.empty()) {
std::unique_lock<std::mutex> lg(m_joint_positon_mutex);
m_current_joint_positions.clear();
if (!m_joint_positon_cv.wait_for(
lg, std::chrono::milliseconds(1000),
[this]() { return !m_current_joint_positions.empty(); })) {
RCLCPP_ERROR(this->get_logger(),
"Joint positions were not received in time");
return;
}
lg.unlock();
// const int max_wait_iterations = 100;
// int wait_count = 0;
// while (m_current_joint_positions.empty() &&
// wait_count++ < max_wait_iterations) {
// std::this_thread::sleep_for(std::chrono::milliseconds(10));
// }
// if (m_current_joint_positions.empty()) {
// RCLCPP_ERROR(this->get_logger(),
// "Joint positions were not received in time");
// return;
// }
std::vector<double> target_joint_values;
solveIK(target_joint_values);
@ -127,6 +141,8 @@ private:
}
// RCLCPP_WARN(this->get_logger(), "Called joint positions");
std::unique_lock<std::mutex> lg(m_joint_positon_mutex);
if (m_joint_mame_to_index.empty()) {
m_joint_mame_to_index.reserve(m_joint_names.size());
for (size_t j = 0; j < m_joint_names.size(); ++j) {
@ -149,6 +165,9 @@ private:
m_current_joint_positions[j] = msg->position[index_it->second];
}
}
lg.unlock();
m_joint_positon_cv.notify_one();
}
std::vector<trajectory_msgs::msg::JointTrajectoryPoint>
@ -233,6 +252,8 @@ private:
std::string m_base_link;
std::string m_ee_link;
std::string m_robot_description;
std::mutex m_joint_positon_mutex;
std::condition_variable m_joint_positon_cv;
};
} // namespace rbs_skill_actions