39 #include <boost/algorithm/string/join.hpp>
44 constexpr
char LOGNAME[] =
"current_state_monitor";
49 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
55 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const ros::NodeHandle& nh)
58 , robot_model_(robot_model)
59 , robot_state_(robot_model)
60 , state_monitor_started_(false)
61 , copy_dynamics_(false)
62 , error_(
std::numeric_limits<double>::
epsilon())
76 return moveit::core::RobotStatePtr(result);
81 const std::vector<const moveit::core::JointModel*>* active_joints = &
robot_model_->getActiveJointModels();
102 joint->getName().c_str());
106 else if (!it->second.isZero())
107 oldest_state_time = std::min(oldest_state_time, it->second);
109 return oldest_state_time;
118 std::pair<moveit::core::RobotStatePtr, ros::Time>
128 std::map<std::string, double> m;
132 for (std::size_t i = 0; i < names.size(); ++i)
133 m[names[i]] = pos[i];
178 if (joint_states_topic.empty())
185 std::make_shared<TFConnection>(
tf_buffer_->_addTransformsChangedListener([
this] { tfCallback(); }));
222 std::vector<std::string>* missing_joints,
223 const std::string& group)
const
225 const std::vector<const moveit::core::JointModel*>* active_joints = &
robot_model_->getActiveJointModels();
236 << std::quoted(group)
237 <<
". All joints of the group are considered to be missing!");
239 *missing_joints =
robot_model_->getActiveJointModelNames();
246 std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it =
joint_time_.find(joint);
251 else if (it->second < oldest_allowed_update_time)
254 joint->getName().c_str(), (oldest_allowed_update_time - it->second).toSec());
259 missing_joints->push_back(joint->getName());
263 return (missing_joints ==
nullptr) || missing_joints->empty();
276 if (elapsed > timeout)
279 "Didn't receive robot state (joint angles) with recent timestamp within "
280 << wait_time <<
" seconds.\n"
281 <<
"Check clock synchronization if your are running ROS across multiple machines!");
290 double slept_time = 0.0;
291 double sleep_step_s = std::min(0.05, wait_time / 10.0);
296 slept_time += sleep_step_s;
303 double slept_time = 0.0;
304 double sleep_step_s = std::min(0.05, wait_time / 10.0);
309 slept_time += sleep_step_s;
311 std::vector<std::string> missing_joints;
322 if (joint_state->name.size() != joint_state->position.size())
326 "State monitor received invalid joint state (number of joint names does not match number of "
335 std::size_t n = joint_state->name.size();
336 for (std::size_t i = 0; i < n; ++i)
345 if (
auto& joint_time{
joint_time_[jm] }; joint_time < joint_state->header.stamp)
347 joint_time = joint_state->header.stamp;
353 <<
"' is not newer than the previous state. Assuming your rosbag looped.");
383 if (joint_state->name.size() == joint_state->velocity.size() &&
391 if (joint_state->name.size() == joint_state->effort.size() &&
404 update_callback(joint_state);
413 const std::vector<const moveit::core::JointModel*>& multi_dof_joints =
robot_model_->getMultiDOFJointModels();
416 bool changes =
false;
422 const std::string& child_frame = joint->getChildLinkModel()->getName();
423 const std::string& parent_frame =
424 joint->getParentLinkModel() ? joint->getParentLinkModel()->getName() :
robot_model_->getModelFrame();
427 geometry_msgs::TransformStamped transf;
431 latest_common_time = transf.header.stamp;
436 << joint->getName() <<
"': Failure to lookup transform between '"
437 << parent_frame.c_str() <<
"' and '" << child_frame.c_str()
438 <<
"' with TF exception: " << ex.what());
447 std::vector<double> new_values(joint->getStateSpaceDimension());
470 sensor_msgs::JointStatePtr joint_state(
new sensor_msgs::JointState);
472 update_callback(joint_state);