43 #include <kdl/tree.hpp>
61 : state_publisher_(rsp), mimic_(m)
68 n_tilde.
param(
"publish_frequency", publish_freq, 50.0);
96 std::string tf_prefix;
99 std::string tf_prefix_key;
101 n_tilde.
param(tf_prefix_key, tf_prefix, std::string(
""));
114 if (state->name.size() != state->position.size()){
115 if (state->position.empty()){
116 const int throttleSeconds = 300;
118 "Robot state publisher ignored a JointState message about joint(s) "
119 "\"%s\"(,...) whose position member was empty. This message will "
120 "not reappear for %d seconds.", state->name[0].c_str(),
123 ROS_ERROR(
"Robot state publisher ignored an invalid JointState message");
132 ROS_WARN(
"Moved backwards in time (probably because ROS clock was reset), re-publishing joint transforms!");
136 if ((state->header.stamp + warning_threshold) < now) {
137 ROS_WARN_THROTTLE(10,
"Received JointState is %f seconds old.", (now - state->header.stamp).toSec());
143 for (
size_t i = 0; i < state->name.size(); ++i) {
145 last_published = (t < last_published) ? t : last_published;
153 std::map<std::string, double> joint_positions;
154 for (
size_t i = 0; i < state->name.size(); ++i) {
155 joint_positions.insert(make_pair(state->name[i], state->position[i]));
158 for (MimicMap::iterator i =
mimic_.begin(); i !=
mimic_.end(); i++) {
159 if(joint_positions.find(i->second->joint_name) != joint_positions.end()) {
160 double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset;
161 joint_positions.insert(make_pair(i->first, pos));
168 for (
size_t i = 0; i<state->name.size(); ++i) {