bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix publish timing for case with multiple joint state sources #4464. Patch from Ingo Kresse.

This commit is contained in:
Wim Meeussen 2011-05-25 10:19:55 -07:00
parent 9c3d8a3de9
commit c70e7505fc
2 changed files with 17 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
robot_state_publisher/include/robot_state_publisher/joint_state_listener.h
View File

@ -68,7 +68,7 @@ private:
robot_state_publisher::RobotStatePublisher state_publisher_; robot_state_publisher::RobotStatePublisher state_publisher_;
Subscriber joint_state_sub_; Subscriber joint_state_sub_;
ros::Timer timer_; ros::Timer timer_;
ros::Time last_publish_time_; std::map<std::string, ros::Time> last_publish_time_;
}; };
} }

19
robot_state_publisher/src/joint_state_listener.cpp
View File

@ -63,7 +63,6 @@ JointStateListener::JointStateListener(const KDL::Tree& tree)
joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this); joint_state_sub_ = n.subscribe("joint_states", 1, &JointStateListener::callbackJointState, this);
// trigger to publish fixed joints // trigger to publish fixed joints
last_publish_time_ = ros::Time();
timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this); timer_ = n_tilde.createTimer(publish_interval_, &JointStateListener::callbackFixedJoint, this);
}; };
@ -85,14 +84,28 @@ void JointStateListener::callbackJointState(const JointStateConstPtr& state)
return; return;
} }
// determine least recently published joint
ros::Time last_published = ros::Time::now();
for (unsigned int i=0; i<state->name.size(); i++)
{
ros::Time t = last_publish_time_[state->name[i]];
last_published = (t < last_published) ? t : last_published;
}
// note: if a joint was seen for the first time,
// then last_published is zero.
// check if we need to publish // check if we need to publish
if (state->header.stamp >= last_publish_time_ + publish_interval_){ if (state->header.stamp >= last_published + publish_interval_){
// get joint positions from state message // get joint positions from state message
map<string, double> joint_positions; map<string, double> joint_positions;
for (unsigned int i=0; i<state->name.size(); i++) for (unsigned int i=0; i<state->name.size(); i++)
joint_positions.insert(make_pair(state->name[i], state->position[i])); joint_positions.insert(make_pair(state->name[i], state->position[i]));
state_publisher_.publishTransforms(joint_positions, state->header.stamp); state_publisher_.publishTransforms(joint_positions, state->header.stamp);
last_publish_time_ = state->header.stamp;
// store publish time in joint map
for (unsigned int i=0; i<state->name.size(); i++)
last_publish_time_[state->name[i]] = state->header.stamp;
} }
} }