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);
94 std::map<std::string, double> m;
98 for (std::size_t i = 0; i < names.size(); ++i)
144 if (joint_states_topic.empty())
151 std::make_shared<TFConnection>(
tf_buffer_->_addTransformsChangedListener([
this] { tfCallback(); }));
188 std::vector<std::string>* missing_joints)
const
190 const std::vector<const moveit::core::JointModel*>& active_joints =
robot_model_->getActiveJointModels();
194 std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it =
joint_time_.find(joint);
199 else if (it->second < oldest_allowed_update_time)
202 joint->getName().c_str(), (oldest_allowed_update_time - it->second).toSec());
208 missing_joints->push_back(joint->getName());
212 return (missing_joints ==
nullptr) || missing_joints->empty();
226 if (elapsed > timeout)
229 "Didn't receive robot state (joint angles) with recent timestamp within "
230 << wait_time <<
" seconds.\n"
231 <<
"Check clock synchronization if your are running ROS across multiple machines!");
240 double slept_time = 0.0;
241 double sleep_step_s = std::min(0.05, wait_time / 10.0);
246 slept_time += sleep_step_s;
258 std::vector<std::string> missing_joints;
264 std::set<std::string> mj;
265 mj.insert(missing_joints.begin(), missing_joints.end());
268 for (std::size_t i = 0;
ok && i < names.size(); ++i)
269 if (mj.find(names[i]) != mj.end())
280 if (joint_state->name.size() != joint_state->position.size())
284 "State monitor received invalid joint state (number of joint names does not match number of "
293 std::size_t n = joint_state->name.size();
295 for (std::size_t i = 0; i < n; ++i)
304 if (
auto& joint_time{
joint_time_[jm] }; joint_time < joint_state->header.stamp)
306 joint_time = joint_state->header.stamp;
312 <<
"' is not newer than the previous state. Assuming your rosbag looped.");
343 if (joint_state->name.size() == joint_state->velocity.size() &&
351 if (joint_state->name.size() == joint_state->effort.size() &&
364 update_callback(joint_state);
373 const std::vector<const moveit::core::JointModel*>& multi_dof_joints =
robot_model_->getMultiDOFJointModels();
376 bool changes =
false;
382 const std::string& child_frame = joint->getChildLinkModel()->getName();
383 const std::string& parent_frame =
384 joint->getParentLinkModel() ? joint->getParentLinkModel()->getName() :
robot_model_->getModelFrame();
387 geometry_msgs::TransformStamped transf;
391 latest_common_time = transf.header.stamp;
396 << joint->getName() <<
"': Failure to lookup transform between '"
397 << parent_frame.c_str() <<
"' and '" << child_frame.c_str()
398 <<
"' with TF exception: " << ex.what());
407 std::vector<double> new_values(joint->getStateSpaceDimension());
430 sensor_msgs::JointStatePtr joint_state(
new sensor_msgs::JointState);
432 update_callback(joint_state);