ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

ROS-I joint_path_command triggers robot only after sending the message twice

asked 2019-01-22 08:34:54 -0500

trhfdntzrtfghdgh gravatar image

updated 2019-01-23 00:12:43 -0500

I am trying to control an ABB 6640 robot through the motion download interface (see here -> 1.3.3 Motion Control -> Subscribed Topics). With a python publisher (built as described here) I publish the following message to the topic /joint_path_command:

 header:
  seq: 0
  stamp:
    secs: 0
    nsecs:         0
  frame_id: ''
joint_names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]
points:
  -
    positions: [0, 0, 0, 0, 0, 0]
    velocities: []
    accelerations: []
    effort: []
    time_from_start:
      secs: 0
      nsecs:         0

I also tried sending the current position of the robot as the first waypoint, but in either way the robot will ignore the first message it is sent. It will only move if the message is send once more after waiting for a little amount of time. Sending the duplicate immediately after the first message will not make it move.

[Edit] The code sending the message:

#!/usr/bin/env python
from trajectory_msgs.msg import *
import rospy

def test():    
    pub = rospy.Publisher('/joint_path_command',JointTrajectory,queue_size=2)
    rospy.init_node('joint_path_command_test',anonymous=True)
    rate = rospy.Rate(1)

    #while not rospy.is_shutdown():
    for i in range(2):
        msg = JointTrajectory()
        msg.joint_names = ["joint_1","joint_2","joint_3","joint_4","joint_5","joint_6"]

        target=JointTrajectoryPoint()
        target.positions=[0,0,0,0,0,0]

        msg.points = [target]
        rospy.loginfo(msg)
        pub.publish(msg)
        rate.sleep()

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

I am running ROS Kinetic Kame in an Ubuntu 16.04 64-bit VM. The Windows host runs a Robot Studio instance with a simulated ABB IRB 6640 robot that was prepared as described in the 3 tutorials here. For controlling it I run roscore and roslaunch abb_irb6640_support robot_interface_download_irb6640_185_280.launch robot_ip:=...

What do I need to change to make the robot listen on the first message sent as well?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-01-22 11:15:32 -0500

gvdhoorn gravatar image

updated 2019-01-23 02:31:28 -0500

Can you please show us some code? I suspect you're sending the message "too soon", which leads to your message getting lost, as no subscriber has connected yet.

This is one of the reasons why using the Action server is preferred / recommended: you are guaranteed to receive feedback.


Edit:

The subscriber is started manually before launching the script - it should have enough time to set up [..]

It's not about the subscriber having time to set up, it's the time needed to establish communication between the publisher and the subscriber.

Your code shows virtually zero time between creating the publisher and publishing the first message, so I would expect it to get lost.

re: get_num_connections(): that should help, yes. Wait until that returns 1 (or as many subscribers that you are expecting / require).

If you're still losing messages, then it could still be that you're publishing too soon. There's nothing specific in the driver that affects any of this, it's all basic roscpp.

edit flag offensive delete link more

Comments

@gvdhoorn I added the full code to my question and will try the action approach. The subscriber is started manually before launching the script - it should have enough time to set up before the first message is sent.

trhfdntzrtfghdgh gravatar image trhfdntzrtfghdgh  ( 2019-01-23 00:16:27 -0500 )edit

Using the /joint_trajectory_action works as expected (also with only one call). @gvdhoorn Before sending my first message in the topic approach, calling pub.get_num_connections() returns 1, so there is a subscriber connected when I do the call. There also should be only one in my setup.

trhfdntzrtfghdgh gravatar image trhfdntzrtfghdgh  ( 2019-01-23 01:08:06 -0500 )edit

So there is no way to check if messages can be sent safely (as I'd expect get_num_connections())? Or any recommended time to wait before sending messages?

trhfdntzrtfghdgh gravatar image trhfdntzrtfghdgh  ( 2019-01-23 02:01:02 -0500 )edit

get_num_connections() should work. Can you show how you are using it?

And it's not a solution, but using the action server is really recommended.


Edit: you don't have a rostopic echo .. running somewhere? That would also count as a subscriber.

gvdhoorn gravatar image gvdhoorn  ( 2019-01-23 02:04:23 -0500 )edit

I'm sorry, there actually was another subscriber (rqt) running... When shutting down all other subscribers than the /motion_download_interface I see 0 subscribers for the first iteration and one for the second one. So get_num_connections seems to work.

trhfdntzrtfghdgh gravatar image trhfdntzrtfghdgh  ( 2019-01-23 02:20:59 -0500 )edit

re: rqt: yes, that will interfere here.

Good to hear that we got things sorted out.

gvdhoorn gravatar image gvdhoorn  ( 2019-01-23 02:32:09 -0500 )edit

Yes, indeed. I wonder why this is not documented. Like this you should expect the official python publisher tutorial (link in question) to not work...
Can I at least be sure that the action will be delivered?

trhfdntzrtfghdgh gravatar image trhfdntzrtfghdgh  ( 2019-01-23 02:39:11 -0500 )edit

What is not documented?

Can I at least be sure that the action will be delivered?

If you set things up properly and use wait_for_server().

gvdhoorn gravatar image gvdhoorn  ( 2019-01-23 02:42:36 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2019-01-22 08:18:03 -0500

Seen: 406 times

Last updated: Jan 23 '19