joint_state_listener.cpp
Go to the documentation of this file.
2 
4 
6  MimicMap &mimic_map, urdf::Model &model, const std::string &urdf)
7 {
8  // gets the location of the robot description on the parameter server
9  bool found;
10  if (urdf.empty())
11  {
12  found = model.initParam("robot_description");
13  if (!found)
14  {
15  ROS_ERROR("Could not read urdf_model from parameter server");
16  return false;
17  }
18  }
19  else
20  {
21  found = model.initString(urdf);
22  if (!found)
23  {
24  ROS_ERROR("Could not read urdf_model from string: %s", urdf.c_str());
25  return false;
26  }
27  }
28 
29  if (!kdl_parser::treeFromUrdfModel(model, tree))
30  {
31  ROS_ERROR("Failed to extract kdl tree from xml robot description");
32  return false;
33  }
34 
35  mimic_map.clear();
36  for (const auto &joint : model.joints_)
37  {
38  if (joint.second->mimic)
39  {
40  mimic_map.insert(make_pair(joint.first, joint.second->mimic));
41  }
42  }
43 
44  return true;
45 }
46 
48  const Tree &tree, const MimicMap &m, const urdf::Model &model)
50 {
51  ROS_INFO("Robot model loaded, it has %lu moving joints and "
52  "%lu fixed joints.", dynamicPublisher.getNumMovingJoints(),
54 
55  dynparamServer.setCallback(boost::bind(
57  this, _1, _2));
58 }
59 
60 
62  const std::string &urdf)
63 {
64  std::lock_guard<std::mutex> lock(updateOngoing);
65 
66  ROS_INFO("Reloading robot model.");
67 
68  timer_.stop();
69  publish_interval_.sleep();
70 
71  KDL::Tree tree;
72  urdf::Model model;
73  mimic_.clear();
74 
75  if (!loadModel(tree, mimic_, model, urdf))
76  return false;
77 
79  timer_.start();
80 
81  ROS_INFO("Robot model reload complete, it has %lu moving joints and "
82  "%lu fixed joints.", dynamicPublisher.getNumMovingJoints(),
84 
85  return true;
86 }
87 
89  const JointStateConstPtr &state)
90 {
91  if (updateOngoing.try_lock())
92  {
94  updateOngoing.unlock();
95  }
96  else
97  {
98  ROS_WARN_THROTTLE(1, "Skipping joint state while robot model is being "
99  "updated.");
100  return;
101  }
102 }
103 
105  DynamicRobotStateConfig &config, uint32_t level)
106 {
107  if (level == 0)
108  reloadRobotModel(config.robot_description);
109 }
void configChangedCb(DynamicRobotStateConfig &config, uint32_t level)
The callback that is called whenever the dynamic robot model changes.
URDF_EXPORT bool initString(const std::string &xmlstring)
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
void callbackJointState(const JointStateConstPtr &state) override
Called when new joint state arrives.
void stop()
void start()
bool reloadRobotModel(const std::string &urdf="")
Reload the robot model.
static bool loadModel(KDL::Tree &tree, MimicMap &mimic_map, urdf::Model &model, const std::string &urdf="")
Parses the given URDF string (or robot_description param if the string is empty) to a KDL tree...
#define ROS_WARN_THROTTLE(period,...)
DynamicRobotStatePublisher dynamicPublisher
The RobotStatePublisher with update functionality.
#define ROS_INFO(...)
robot_state_publisher::RobotStatePublisher state_publisher_
DynamicJointStateListener(const Tree &tree, const MimicMap &m, const urdf::Model &model)
Construct the listener using the given KDL tree, URDF model and mimic map.
URDF_EXPORT bool initParam(const std::string &param)
dynamic_reconfigure::Server< DynamicRobotStateConfig > dynparamServer
The dynamic parameter server.
virtual size_t getNumFixedJoints() const
Return the number of fixed joints in the currently represented model.
virtual void updateTree(const KDL::Tree &tree)
Sets the robot model.
std::map< std::string, urdf::JointMimicSharedPtr > MimicMap
virtual void callbackJointState(const JointStateConstPtr &state)
#define ROS_ERROR(...)
virtual size_t getNumMovingJoints() const
Return the number of moving joints in the currently represented model.


dynamic_robot_state_publisher
Author(s): Martin Pecka
autogenerated on Mon Mar 18 2019 02:25:19