39 #include <kdl/tree.hpp> 50 : state_publisher_(tree, model), mimic_(m)
57 n_tilde.
param(
"publish_frequency", publish_freq, 50.0);
63 std::string tf_prefix_key;
94 if (state->name.size() != state->position.size()){
95 if (state->position.empty()){
96 const int throttleSeconds = 300;
98 "Robot state publisher ignored a JointState message about joint(s) " 99 "\"%s\"(,...) whose position member was empty. This message will " 100 "not reappear for %d seconds.", state->name[0].c_str(),
103 ROS_ERROR(
"Robot state publisher ignored an invalid JointState message");
112 ROS_WARN(
"Moved backwards in time (probably because ROS clock was reset), re-publishing joint transforms!");
116 if ((state->header.stamp + warning_threshold) < now) {
117 ROS_WARN_THROTTLE(10,
"Received JointState is %f seconds old.", (now - state->header.stamp).toSec());
123 for (
unsigned int i=0; i<state->name.size(); i++) {
125 last_published = (t < last_published) ? t : last_published;
133 map<string, double> joint_positions;
134 for (
unsigned int i=0; i<state->name.size(); i++) {
135 joint_positions.insert(make_pair(state->name[i], state->position[i]));
138 for (MimicMap::iterator i =
mimic_.begin(); i !=
mimic_.end(); i++) {
139 if(joint_positions.find(i->second->joint_name) != joint_positions.end()) {
140 double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset;
141 joint_positions.insert(make_pair(i->first, pos));
148 for (
unsigned int i = 0; i<state->name.size(); i++) {
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
~JointStateListener()
Destructor.
Duration publish_interval_
virtual void publishFixedTransforms(const std::string &tf_prefix, bool use_tf_static=false)
ros::Time last_callback_time_
virtual void callbackFixedJoint(const ros::TimerEvent &e)
Subscriber joint_state_sub_
std::map< std::string, ros::Time > last_publish_time_
TransportHints & tcpNoDelay(bool nodelay=true)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
virtual void publishTransforms(const std::map< std::string, double > &joint_positions, const ros::Time &time, const std::string &tf_prefix)
#define ROS_WARN_THROTTLE(period,...)
robot_state_publisher::RobotStatePublisher state_publisher_
bool searchParam(const std::string &key, std::string &result) const
std::map< std::string, urdf::JointMimicSharedPtr > MimicMap
virtual void callbackJointState(const JointStateConstPtr &state)
double max(double a, double b)