Publishing joint_state with python and displaying in rviz

Question

Hey Guys,

I'm have a robot arm I'm trying to put into ros. I went through the urdf_tutorial and got a urdf that works with the tutorial launch file. Now, I want to write a node that publishes to /joint_states and uses robot_state_publisher to update the robot. I'd like to display the robot in rviz, and also write a subscriber to /joint_states that moves the actual arm.

The problem I'm having is, when I write my own simple node to publish to /joint_states, rviz gives me errors for all of my joint transforms. (ie "no transform from [first_segment] to [base_link]" and "no transform from [second_segment] to [base_link]", also there's "Fixed Frame: No tf data. Actual error: Fixed Frame [base_link] does not exist"

I've looked at the documentation for robot_state_publisher, read the code for joint_state_publisher and tried manually sending tf's through my node.

I use this command to put in my urdf and launch the code:

roslaunch launch/display.launch model:=urdf/josh-arm.urdf

Here's my urdf, launch file, and python code (sorry I couldn't attach it.. don't have enough karma):

Thanks!

<launch>

  <arg name="model" />
  <arg name="gui" default="true" />
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />

  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
  <param name="use_gui" value="$(arg gui)"/>

  <node name="updateArm" pkg="me439arm" type="updateArm.py"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

</launch>

The urdf file:

<?xml version="1.0"?>
<robot name="visual">

<material name="blue">
    <color rgba="0 0 0.8 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>

  <link name="base_link">
    <visual>
      <geometry>
        <cylinder length="0.1026" radius="0.025"/>
      </geometry>
    </visual>
  </link>

  <link name="base_rotate">
    <visual>
      <geometry>
    <sphere radius="0.025"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 .1026"/>
      <material name="white"/>
    </visual>
  </link>


  <joint name="base_joint" type="revolute">
    <axis xyz=" 0 0 1"/>
    <parent link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <limit effort="1000.0" lower="-3.14159" upper="3.14159" velocity=".5"/>
    <child link="base_rotate"/>
  </joint>

  <link name="first_segment">
    <visual>
      <geometry>
    <cylinder length="0.1180" radius="0.025"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-.0310 0 .059"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="first_segment_joint" type="revolute">
    <axis rpy="0 1 0" xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 .1"/>
    <limit effort="1000.0" lower="-3.14159" upper="3.14159" velocity=".5"/>
    <parent link="base_rotate"/>
    <child link="first_segment"/>
  </joint>

  <link name="second_segment">
    <visual>
      <geometry>
    <cylinder length="0.02" radius="0.025"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 .01"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="second_segment_joint" type="revolute">
    <axis rpy="0 1 0" xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="-.0310 0 .1180"/>
    <limit effort="1000.0" lower="-3.14159" upper="3.14159" velocity=".5"/>
    <parent link="first_segment"/>
    <child link="second_segment"/>
  </joint>

  <link name="third_segment">
    <visual>
      <geometry>
    <cylinder length=".1335" radius="0.025"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.06675"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="third_segment_joint" type="revolute">
    <axis rpy="0 0 1" xyz="0 0 1"/>
    <origin rpy="0 -1.570796 0" xyz="0 0 .02"/>
    <limit effort="1000.0" lower="-3.14159" upper="3.14159" velocity=".5"/>
    <parent link="second_segment"/>
    <child link="third_segment"/>
  </joint>

  <link name="wrist_segment1">
    <visual>
      <geometry>
    <cylinder length="0.029" radius="0.025"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.0145"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="wrist_segment_joint1" type="revolute">
    <axis rpy="0 0 1" xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 .1335"/>
    <limit effort="1000.0" lower="-3.14159" upper="3.14159" velocity=".5"/>
    <parent link="third_segment"/>
    <child link="wrist_segment1"/>
  </joint>

  <link name="wrist_segment2">
    <visual>
      <geometry>
    <cylinder length="0.001" radius="0.025"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="white"/>
    </visual>
  </link>  

  <joint name="wrist_segment_joint2" type="revolute">
    <axis rpy="0 0 1" xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 .025"/>
    <limit effort="1000.0" lower="-3.14159" upper="3.14159" velocity=".5"/>
    <parent link="wrist_segment1"/>
    <child link="wrist_segment2"/>
  </joint>

  </robot>

The Python file (based on the publisher tutorial):

#!/usr/bin/env python
# license removed for brevity
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import JointState
import tf

br = tf.TransformBroadcaster();
def get_param(name, value=None):
    private = "~%s" % name
    if rospy.has_param(private):
        return rospy.get_param(private)
    elif rospy.has_param(name):
        return rospy.get_param(name)
    else:
        return value

def talker():
    pub = rospy.Publisher('joint_states', JointState, queue_size=10)
    rospy.init_node('updateArm', anonymous=True)
    rate = rospy.Rate(10) # 10hz
    msg = JointState()
    msg.name = ['base_joint', 'first_segment_joint', 'second_segment_joint', 'third_segment_joint', 'wrist_segment_joint1', 'wrist_segment_joint2'];
    msg.position = [0,0,0,0,0,0]
    
    source_list = get_param("source_list", [])
    rospy.loginfo(str(source_list))
    while not rospy.is_shutdown():
        msg.header.stamp = rospy.Time.now();
        pub.publish(msg)
        rate.sleep()

if __name__ == '__main__':
    try:
        talker()
    except rospy.ROSInterruptException:
        pass

---

Originally posted by jtabor on ROS Answers with karma: 3 on 2017-11-08

Post score: 0

---

Original comments

Comment by jayess on 2017-11-09:
Don't worry about not being able to your code as a file. It's actually better to include the code in the question as you have done here.

Answer 1

A robot driver (but that is what we're talking about here) typically only needs to publish JointState messages, the rest will be taken care of by the robot_state_publisher (using forward kinematics).

An exception to this would be if you have 'strange' joint types which cannot be modelled properly in urdf or for which it doesn't make sense to go through JointState msgs first: then the driver would have to publish the corresponding TF frames itself (and the joints should not be part of the urdf in that case).

So having a robot_description parameter with a valid urdf, your driver + RSP should be enough to make RViz happy (and result in a working system, at least for displaying robot state).

and also write a subscriber to /joint_states that moves the actual arm.

I'm a little confused by this: /joint_states is for reporting state of your robot, not for controlling it.

---

Edit:

Okay, so i was using joint_state_publisher before to change the arms location on rviz, but that's only changing what's reported by the robot?

Well, that depends on what you consider "the robot": if you're just playing with your urdf, then you typically use the JSP to publish some fake JointState msgs where the values are taken from the sliders in the GUI.

With a real robot you would not use this, as your driver would be the entity publishing JointState msgs.

How can I control the joint angles? Is there a way to set joint angles and have RSP calculate the tf's for me? T

The RSP will always calculate TFs for you, provided that you got your urdf loaded and you have JointState msgs published by 'something' (ie: your driver). That is not influenced by whether or not you can control your robot.

Typical ROS robot drivers (for arms at least, but I'm guessing that is what you have) could implement a FollowJoinTrajectory action server. That would accept a JointTrajectory together with some constraints and would translate that into motions of the arm corresponding to what the trajectory prescribes (in space and time).

If you're writing a new driver you could perhaps take a look at ros_control. It's a framework that comes with a lot of infrastructure already so removes the need to develop things like that FollowJointTrajectory action server, controllers and state publishers. The 'only' thing you need to do is write a small hardware_interface class that allows ros_control to read out state and write new setpoints to your robot (and/or its controller).

ros_control is a bit of an advanced subject (and C++ only at the moment), so might be out of scope for now. I just wanted to mention it as it is a good example of an area where software re-use can really speed up your development.

---

Originally posted by gvdhoorn with karma: 86574 on 2017-11-09

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by jtabor on 2017-11-09:
Okay, so i was using joint_state_publisher before to change the arms location on rviz, but that's only changing what's reported by the robot? How can I control the joint angles? Is there a way to set joint angles and have RSP calculate the tf's for me? That's what I really want. Thanks!

Comment by jtabor on 2017-11-09:
This is really awesome. I think my application is strange because we use servos for the arm, so there's no feedback (this is just for a class I teach). Writing a hardware interface shouldn't be a problem, and the JointTrajectories seem pretty straightforward. Thanks again!

Comment by gvdhoorn on 2017-11-09:
If you're going with ros_control, you might want to take a look at Dave Coleman's ros_control_boilerplate. It includes even more of the 'standard' stuff, so you have to do even less (fill in read() and write()).

Comment by gvdhoorn on 2017-11-09:\

I think my application is strange because we use servos for the arm, so there's no feedback

doesn't matter. The principles remain the same.

ros_control includes a few forwarding command controllers, which don't do any closed loop control, just forward setpoints. Those would probably ..

Comment by gvdhoorn on 2017-11-09:
.. work nicely with your servos.

I'd also recommend trying to find some hardware_interface implementations for Dynamixels (and similar servos). There are a few and you could probably learn from those.

Comment by jtabor on 2017-11-09:
Thanks! I'll check out the hardware_interfaces and the forward commanding controller. We've wrote the whole control code in python before, so I think using ros for some of it will be a huge upgrade. Especially since we can do a lot more in simulation, and we're teaching it to them anyway.