59 lines
1.6 KiB
C++
59 lines
1.6 KiB
C++
#include <string>
|
|
#include <ros/ros.h>
|
|
#include <sensor_msgs/JointState.h>
|
|
#include <urdf/model.h>
|
|
#include <urdf/joint.h>
|
|
#include <map>
|
|
|
|
int main(int argc, char** argv) {
|
|
ros::init(argc, argv, "easy_state_publisher");
|
|
ros::NodeHandle n;
|
|
ros::Publisher m_joint_pub = n.advertise
|
|
<sensor_msgs::JointState>("joint_states", 1);
|
|
ros::Rate loop_rate(2);
|
|
|
|
urdf::Model my_model;
|
|
if (!my_model.initParam("/robot_description")) {
|
|
ROS_ERROR("Failed to parse urdf robot model");
|
|
return false;
|
|
}
|
|
|
|
typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > joint_map;
|
|
typedef std::map<std::string, double> position_map;
|
|
joint_map joints = my_model.joints_;
|
|
position_map positions;
|
|
|
|
int count = 0;
|
|
for (joint_map::const_iterator it = joints.begin(); it != joints.end(); ++it) {
|
|
std::string name = it->first;
|
|
urdf::Joint* joint = it->second.get();
|
|
if (joint->type!=urdf::Joint::FIXED) {
|
|
positions[name] = 0;
|
|
count++;
|
|
ROS_INFO("%s", name.c_str());
|
|
}
|
|
}
|
|
|
|
sensor_msgs::JointState joint_state;
|
|
joint_state.name.resize(count);
|
|
joint_state.position.resize(count);
|
|
joint_state.velocity.resize(count);
|
|
int i = 0;
|
|
for (position_map::const_iterator it = positions.begin(); it != positions.end(); ++it) {
|
|
joint_state.name[i++] = it->first.c_str();
|
|
}
|
|
|
|
while (ros::ok()) {
|
|
|
|
joint_state.header.stamp = ros::Time::now();
|
|
m_joint_pub.publish(joint_state);
|
|
|
|
|
|
// This will adjust as needed per iteration
|
|
loop_rate.sleep();
|
|
}
|
|
|
|
|
|
return 0;
|
|
}
|