12 found = model.
initParam(
"robot_description");
15 ROS_ERROR(
"Could not read urdf_model from parameter server");
24 ROS_ERROR(
"Could not read urdf_model from string: %s", urdf.c_str());
31 ROS_ERROR(
"Failed to extract kdl tree from xml robot description");
36 for (
const auto &joint : model.joints_)
38 if (joint.second->mimic)
40 mimic_map.insert(make_pair(joint.first, joint.second->mimic));
51 ROS_INFO(
"Robot model loaded, it has %lu moving joints and " 62 const std::string &
urdf)
81 ROS_INFO(
"Robot model reload complete, it has %lu moving joints and " 105 DynamicRobotStateConfig &config, uint32_t level)
void configChangedCb(DynamicRobotStateConfig &config, uint32_t level)
The callback that is called whenever the dynamic robot model changes.
std::mutex updateOngoing
Mutex for updates.
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.
bool reloadRobotModel(const std::string &urdf="")
Reload the robot model.
Duration publish_interval_
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.
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 ¶m)
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)
virtual size_t getNumMovingJoints() const
Return the number of moving joints in the currently represented model.