45 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
51 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
72 robot_state::RobotState* result =
new robot_state::RobotState(
robot_state_);
73 return robot_state::RobotStatePtr(result);
82 std::pair<robot_state::RobotStatePtr, ros::Time>
86 robot_state::RobotState* result =
new robot_state::RobotState(
robot_state_);
92 std::map<std::string, double> m;
95 const std::vector<std::string>& names =
robot_state_.getVariableNames();
96 for (std::size_t i = 0; i < names.size(); ++i)
104 const double* pos =
robot_state_.getVariablePositions();
105 upd.setVariablePositions(pos);
110 const double* vel =
robot_state_.getVariableVelocities();
111 upd.setVariableVelocities(vel);
115 const double* acc =
robot_state_.getVariableAccelerations();
116 upd.setVariableAccelerations(acc);
121 upd.setVariableEffort(eff);
142 if (joint_states_topic.empty())
143 ROS_ERROR(
"The joint states topic cannot be an empty string");
172 ROS_DEBUG(
"No longer listening for joint states");
188 const std::vector<const moveit::core::JointModel*>& joints =
robot_model_->getActiveJointModels();
193 if (!joint->isPassive() && !joint->getMimic())
195 ROS_DEBUG(
"Joint '%s' has never been updated", joint->getName().c_str());
205 const std::vector<const moveit::core::JointModel*>& joints =
robot_model_->getActiveJointModels();
209 if (!joint->isPassive() && !joint->getMimic())
211 missing_states.push_back(joint->getName());
220 const std::vector<const moveit::core::JointModel*>& joints =
robot_model_->getActiveJointModels();
226 if (joint->isPassive() || joint->getMimic())
228 std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it =
joint_time_.find(joint);
231 ROS_DEBUG(
"Joint '%s' has never been updated", joint->getName().c_str());
234 else if (it->second < old)
236 ROS_DEBUG(
"Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
237 joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
245 std::vector<std::string>& missing_states)
const 248 const std::vector<const moveit::core::JointModel*>& joints =
robot_model_->getActiveJointModels();
254 if (joint->isPassive() || joint->getMimic())
256 std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it =
joint_time_.find(joint);
259 ROS_DEBUG(
"Joint '%s' has never been updated", joint->getName().c_str());
260 missing_states.push_back(joint->getName());
263 else if (it->second < old)
265 ROS_DEBUG(
"Joint '%s' was last updated %0.3lf seconds ago (older than the allowed %0.3lf seconds)",
266 joint->getName().c_str(), (now - it->second).toSec(), age.toSec());
267 missing_states.push_back(joint->getName());
285 if (elapsed > timeout)
287 ROS_INFO_STREAM(
"Didn't received robot state (joint angles) with recent timestamp within " 288 << wait_time <<
" seconds.\n" 289 <<
"Check clock synchronization if your are running ROS across multiple machines!");
298 double slept_time = 0.0;
299 double sleep_step_s = std::min(0.05, wait_time / 10.0);
304 slept_time += sleep_step_s;
316 std::vector<std::string> missing_joints;
322 std::set<std::string> mj;
323 mj.insert(missing_joints.begin(), missing_joints.end());
326 for (std::size_t i = 0; ok && i < names.size(); ++i)
327 if (mj.find(names[i]) != mj.end())
338 if (joint_state->name.size() != joint_state->position.size())
340 ROS_ERROR_THROTTLE(1,
"State monitor received invalid joint state (number of joint names does not match number of " 349 std::size_t n = joint_state->name.size();
351 for (std::size_t i = 0; i < n; ++i)
362 if (
robot_state_.getJointPositions(jm)[0] != joint_state->position[i])
365 robot_state_.setJointPositions(jm, &(joint_state->position[i]));
369 if (static_cast<const moveit::core::RevoluteJointModel*>(jm)->isContinuous())
372 const robot_model::VariableBounds& b =
377 if (joint_state->position[i] < b.min_position_ && joint_state->position[i] >= b.min_position_ -
error_)
379 else if (joint_state->position[i] > b.max_position_ && joint_state->position[i] <= b.max_position_ +
error_)
387 if (joint_state->name.size() == joint_state->velocity.size() &&
391 robot_state_.setJointVelocities(jm, &(joint_state->velocity[i]));
395 if (joint_state->name.size() == joint_state->effort.size() &&
399 robot_state_.setJointEfforts(jm, &(joint_state->effort[i]));
417 const std::vector<const moveit::core::JointModel*>& multi_dof_joints =
robot_model_->getMultiDOFJointModels();
420 bool changes =
false;
424 for (
size_t i = 0; i < multi_dof_joints.size(); i++)
428 const std::string& parent_frame =
432 geometry_msgs::TransformStamped transf;
436 latest_common_time = transf.header.stamp;
441 << joint->
getName() <<
"': Failure to lookup transform between '" << parent_frame.c_str()
442 <<
"' and '" << child_frame.c_str() <<
"' with TF exception: " << ex.what());
453 if (link->jointOriginTransformIsIdentity())
464 robot_state_.setJointPositions(joint, new_values.data());
474 sensor_msgs::JointStatePtr joint_state(
new sensor_msgs::JointState);
boost::signals2::connection TFConnection
void setToCurrentState(robot_state::RobotState &upd) const
Set the state upd to the current state maintained by this class.
void stopStateMonitor()
Stop monitoring the "joint_states" topic.
bool state_monitor_started_
bool haveCompleteState() const
Query whether we have joint state information for all DOFs in the kinematic model.
std::string getTopic() const
ros::Time getCurrentStateTime() const
Get the time stamp for the current state.
boost::mutex state_update_lock_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::map< const moveit::core::JointModel *, ros::Time > joint_time_
bool isActive() const
Check if the state monitor is started.
Monitors the joint_states topic and tf to maintain the current state of the robot.
robot_model::RobotModelConstPtr robot_model_
const std::vector< std::string > & getJointModelNames() const
CurrentStateMonitor(const robot_model::RobotModelConstPtr &robot_model, const std::shared_ptr< tf2_ros::Buffer > &tf_buffer)
Constructor.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
void startStateMonitor(const std::string &joint_states_topic="joint_states")
Start monitoring joint states on a particular topic.
boost::condition_variable state_update_condition_
virtual double distance(const double *value1, const double *value2) const=0
std::vector< JointStateUpdateCallback > update_callbacks_
ros::Time current_state_time_
robot_state::RobotState robot_state_
void jointStateCallback(const sensor_msgs::JointStateConstPtr &joint_state)
#define ROS_ERROR_THROTTLE(period,...)
virtual void computeVariablePositions(const Eigen::Isometry3d &transform, double *joint_values) const=0
std::size_t getVariableCount() const
std::string resolveName(const std::string &name, bool remap=true) const
const LinkModel * getChildLinkModel() const
const std::string & getName() const
std::shared_ptr< TFConnection > tf_connection_
std::shared_ptr< tf2_ros::Buffer > tf_buffer_
ros::Subscriber joint_state_subscriber_
bool waitForCurrentState(const ros::Time t=ros::Time::now(), double wait_time=1.0) const
Wait for at most wait_time seconds (default 1s) for a robot state more recent than t...
const VariableBounds & getVariableBounds(const std::string &variable) const
#define ROS_INFO_STREAM(args)
robot_state::RobotStatePtr getCurrentState() const
Get the current state.
bool waitForCompleteState(double wait_time) const
Wait for at most wait_time seconds until the complete robot state is known.
#define ROS_WARN_STREAM_ONCE(args)
void clearUpdateCallbacks()
Clear the functions to be called when an update to the joint state is received.
boost::function< void(const sensor_msgs::JointStateConstPtr &joint_state)> JointStateUpdateCallback
std::map< std::string, double > getCurrentStateValues() const
Get the current state values as a map from joint names to joint state values.
ros::Time monitor_start_time_
const LinkModel * getParentLinkModel() const
JointType getType() const
std::string getMonitoredTopic() const
Get the name of the topic being monitored. Returns an empty string if the monitor is inactive...
void addUpdateCallback(const JointStateUpdateCallback &fn)
Add a function that will be called whenever the joint state is updated.
virtual unsigned int getStateSpaceDimension() const=0
std::pair< robot_state::RobotStatePtr, ros::Time > getCurrentStateAndTime() const
Get the current state and its time stamp.
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
const std::string & getName() const