trying to use get joint state with my urdf

Question

hi i am trying to use the jointstates topic of pr2 for my own urdf file, i am not sure how should i start. how do i get my robot to publish on that topic? do i need to write a code for a publisher of my own? (very new in ros & c++)

thanks everyone

so now i found this link- http://ros.org/wiki/joint_state_publisher but istill don't understand how to work it. somehow these things aren't working for me :(

---

Originally posted by Nachum on ROS Answers with karma: 208 on 2012-09-05

Post score: 0

---

Original comments

Comment by Lorenz on 2012-09-05:
Simulation or a real robot?

Comment by Nachum on 2012-09-06:
Simulation

Answer 1

To get joint states, a node that communicates with your robot's hardware is run on the robot. It reads the position of the encoders in all joints and constructs a sensor_msgs/JointState message that is then published on the /joint_states topic.

If you are running on a real robot, you need to implement such a node. On the PR2 that's the pr2_etherCAT node and you need to implement something similar if you are using a custom robot and want to have joint states.

In simulation, things can be a little easier because you can use the pr2 controller manager that's used on the simulated pr2 for other robots, too The gazebo_ros_controller_manager plugin is responsible for providing the infrastructure for running controllers and for publishing the joint state. You should be able to use it in a custom urdf. I think you will have to add transmission specifications though which seem to be undocumented. To use the controller plugin, add the following to your urdf file:

  <gazebo>
    <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>1000.0</updateRate>
      <interface:audio name="gazebo_ros_controller_manager_dummy_iface" />
    </controller:gazebo_ros_controller_manager>
  </gazebo>

If you do not want to use that plugin, you need to write your own gazebo plugin that reads the positions of the joints specified in your URDF file, advertises the /joint_states topic and publishes the joint positions as a message of type sensor_msgs/JointState.

The joint_state_publisher node you mentioned in your question essentially publishes a constant value for all non-fixed joint positions, i.e. it does not respect the actual position of joints in simulation. It also provides a gui that lets you to change the joint state values. This node is most useful when you are creating a urdf file to check if all joints are aligned correctly.

---

Originally posted by Lorenz with karma: 22731 on 2012-09-06

This answer was ACCEPTED on the original site

Post score: 6

---

Original comments

Comment by Nachum on 2012-09-10:
o.k I understood how to run it now. the position and velocitiy look correct but the effort is giving zero when that can't be. did i miss some thing? Thanks

Answer 2

well, if you want to publish the joint state by yourself, try the following code:

#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
ros::init(argc, argv, "myRobot_move_joint");
ros::NodeHandle n;
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
tf::TransformBroadcaster broadcaster;
ros::Rate loop_rate(30);

const double degree = M_PI/180;
double rot4 = 90;

geometry_msgs::TransformStamped odom_trans;
sensor_msgs::JointState joint_state;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

joint_state.name.resize(7);
joint_state.position.resize(7);
joint_state.name[0] ="base_to_left_link1";
joint_state.name[1] ="left_link1_to_left_link2";
joint_state.name[2] ="left_link2_to_left_link3";
joint_state.name[3] ="left_link3_to_left_link4";
joint_state.name[4] ="left_link4_to_left_link5";
joint_state.name[5] ="left_link5_to_left_link6";
joint_state.name[6] ="left_link6_to_left_link7";

while (ros::ok()) {
    //update joint_state
    joint_state.header.stamp = ros::Time::now();
    joint_state.position[0] = 0;
    joint_state.position[1] = 0;
    joint_state.position[2] = 0;
    joint_state.position[3] = rot4*degree;
    joint_state.position[4] = 0;
    joint_state.position[5] = 0;
    joint_state.position[6] = 0;

    // update transform
    // (moving in a circle with radius=2)
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.transform.translation.x = 0;
    odom_trans.transform.translation.y = 0;
    odom_trans.transform.translation.z = 0;
    odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(0);

    //send the joint state and transform
    joint_pub.publish(joint_state);
    broadcaster.sendTransform(odom_trans);

    rot4 += 1;
    if (rot4 > 90) rot4 = 0;

    loop_rate.sleep();
}
return 0;
}

this works for my 7DOF robot arm. just change the joint names to your own.

---

Originally posted by yangyangcv with karma: 741 on 2012-09-06

This answer was NOT ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Lorenz on 2012-09-06:
Isn't this just publishing a constant joint state? It seems to ignore the actual positions of the robot's joint. Also, it seems like ros::spinOnce() is missing so the node should not really behave correctly.

Comment by yangyangcv on 2012-09-06:
for me, i have another program getting the robot's actual joint position(running on windows). so by feeding in the corresponding joint positions to the above code, i can publish the joint states to other nodes in ROS. i'm not very clear about the effect of spinonce(), but the above code do work.

Comment by yangyangcv on 2012-09-06:
and, according to my understanding, we only need the spinonce() when the node subscribe to other node's topic. am i correct?

Comment by Lorenz on 2012-09-06:
You are right, it seems like ros::spinOnce is not needed here. I was not sure if node commands such as rosnode info and rosnode kill work without spinOnce. I made a test and apparently they do :)

Comment by Nachum on 2012-09-06:
thanks. i hope i will be able to run it :)

Comment by Nachum on 2012-09-06:
some errors- tried to make it: fatal error: tf/transform_broadcaster.h: No such file or directory compilation terminated.

Comment by Lorenz on 2012-09-06:
You need to add a dependency on tf in your manifest.xml file.

Comment by yangyangcv on 2012-09-06:
@Nachum: seems you have lots to learn about ROS. open a new terminal, cd into a folder which is in your ROS_PACKAGE_PATH, type: roscreate-pkg my_robot_publish_joint_goal roscpp tf sensor_msgs this will create a package which depends on roscpp, tf and sensor_msgs

Comment by yangyangcv on 2012-09-06:
maybe you need to learn the basic tutorials first, for example this one http://www.ros.org/wiki/ROS/Tutorials/CreatingPackage

Comment by Nachum on 2012-09-06:
it is publishing but wrong. my pendelum is swaying and it is giving me zero on all parameters

Comment by Lorenz on 2012-09-06:
What did you expect? The code above sets all joint states but one to zero and always publishes the same message. You of course need to fill in the joint state values you read out from simulation.