39 #include <kdl/tree.hpp> 51 : state_publisher_(tree, model), mimic_(m)
58 n_tilde.
param(
"publish_frequency", publish_freq, 50.0);
64 std::string tf_prefix_key;
95 if (state->name.size() != state->position.size()){
96 if (state->position.empty()){
97 const int throttleSeconds = 300;
99 "Robot state publisher ignored a JointState message about joint(s) " 100 "\"%s\"(,...) whose position member was empty. This message will " 101 "not reappear for %d seconds.", state->name[0].c_str(),
104 ROS_ERROR(
"Robot state publisher ignored an invalid JointState message");
113 ROS_WARN(
"Moved backwards in time (probably because ROS clock was reset), re-publishing joint transforms!");
117 if ((state->header.stamp + warning_threshold) < now) {
118 ROS_WARN_THROTTLE(10,
"Received JointState is %f seconds old.", (now - state->header.stamp).toSec());
124 for (
unsigned int i=0; i<state->name.size(); i++) {
126 last_published = (t < last_published) ? t : last_published;
134 map<string, double> joint_positions;
135 for (
unsigned int i=0; i<state->name.size(); i++) {
136 joint_positions.insert(make_pair(state->name[i], state->position[i]));
139 for (MimicMap::iterator i =
mimic_.begin(); i !=
mimic_.end(); i++) {
140 if(joint_positions.find(i->second->joint_name) != joint_positions.end()) {
141 double pos = joint_positions[i->second->joint_name] * i->second->multiplier + i->second->offset;
142 joint_positions.insert(make_pair(i->first, pos));
149 for (
unsigned int i = 0; i<state->name.size(); i++) {
158 int main(
int argc,
char** argv)
161 ros::init(argc, argv,
"robot_state_publisher");
165 std::string exe_name = argv[0];
166 std::size_t slash = exe_name.find_last_of(
"/");
167 if (slash != std::string::npos) {
168 exe_name = exe_name.substr(slash + 1);
170 if (exe_name ==
"state_publisher") {
171 ROS_WARN(
"The 'state_publisher' executable is deprecated. Please use 'robot_state_publisher' instead");
177 if (!model.
initParam(
"robot_description"))
182 ROS_ERROR(
"Failed to extract kdl tree from xml robot description");
188 for(std::map< std::string, urdf::JointSharedPtr >::iterator i = model.joints_.begin(); i != model.joints_.end(); i++) {
189 if(i->second->mimic) {
190 mimic.insert(make_pair(i->first, i->second->mimic));
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
~JointStateListener()
Destructor.
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Duration publish_interval_
virtual void publishFixedTransforms(const std::string &tf_prefix, bool use_tf_static=false)
ros::Time last_callback_time_
int main(int argc, char **argv)
virtual void callbackFixedJoint(const ros::TimerEvent &e)
Subscriber joint_state_sub_
ROSCPP_DECL void spin(Spinner &spinner)
std::map< std::string, ros::Time > last_publish_time_
TransportHints & tcpNoDelay(bool nodelay=true)
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,...)
bool searchParam(const std::string &key, std::string &result) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
robot_state_publisher::RobotStatePublisher state_publisher_
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
URDF_EXPORT bool initParam(const std::string ¶m)
std::map< std::string, urdf::JointMimicSharedPtr > MimicMap
virtual void callbackJointState(const JointStateConstPtr &state)
double max(double a, double b)