Unstable Communication between ROS Node and Robot
Hey ROS Forum,
I am just trying to make my robot navigate from a initial to a goal position. The node then calculates the next action in form of the angular rotation and continuous calculating and observing the state. Here the computing time for one step mainly depends on the waiting time for the Lidar data (5 data sets per second).
data = rospy.wait_for_message('scan', LaserScan, timeout=5)
Based on that data, the algo calculates the next action and publishes it. Here the problem occurs, if I just write
self.pub_cmd_vel.publish(vel_cmd)
the robot misses that published topic and continuous following an old action. I tried to use an additional node that permanently subscribes to the first node and also publishes to the robot (first fast design):
if __name__ == '__main__':
rospy.init_node('turtlebot3_signal_transmission')
pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
vel_cmd = Twist()
t = True
action = rospy.wait_for_message('cmd_vel_1', Twist)
while t:
a = time.time()
time_diff = 0
while time_diff < 0.2:
vel_cmd.linear.x = action.linear.x; vel_cmd.angular.z = action.angular.z
pub_cmd_vel.publish(vel_cmd)
action = rospy.Subscriber('cmd_vel_1', Twist, queue_size=10)
time_diff = time.time() - a
But here, when reaching the second while loop, the subscriber delivers the action with the following value:
<rospy.topics.Subscriber object at 0x7fd66c79b210>
My question is now, how can I ensure a continuos communication, where the actual value for the action is permanently published and subscribed by the middle node? If I put the waitformessage into the while loop, the whole procedure of receiving and sending lasts 0,4sec instead of 0,2 sec. The goal would be to have a robot action_update rate of 0,2 sec as well. I also tried the
self.pub_cmd_vel.get_num_connections()
command, but it wasn`t fully working due to a constant 1 as a value after first execution of the node.
Thanks in advance!
Asked by Tima1995 on 2019-10-20 08:12:28 UTC
Answers
I think you misunderstood how subscribers works because you made a little confusion about how to receive the data.
The first time using rospy.wait_for_message('cmd_vel_1', Twist) the variable action is propperly assigned the value of the message from the topic cmd_vel but then with this line :
action = rospy.Subscriber('cmd_vel_1', Twist, queue_size=10)
You don't get the message from cmd_vel_1 like that. This defines the subscriber to the cmd_vel_1 topic and you can get the data using a callback function (which you don't define when declaring the subscriber). So the varaible action is not correctly set.
You should use the same template as in the tutorials. Simply create a callback function when declaring the subscriber (you should replace the wait_for_message line with the declaration of the subscriber) and it should be working fine.
Asked by Delb on 2019-10-21 02:39:08 UTC
Comments
Hello Delb and thanks for the answer! The point for me is, that the navigation node that publishes the action needs in average 0,2 secs between publishing to vel_cmd_1 topics. That's why I tried to introduce the time diff and used the wait_for_message, to get always the latest action command. But I saw my mistakes there. I tried to write a new one but still, it does not work properly:
def transmission():
rospy.init_node('turtlebot3_signal_transmission')
pub_cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=5)
vel_cmd = Twist()
time_1 = 0
time_2 = 0
while not rospy.is_shutdown():
action = rospy.wait_for_message('cmd_vel_1', Twist)
vel_cmd.linear.x = action.linear.x; vel_cmd.angular.z = action.angular.z
time_1 = time.time()
time_2 = time.time()
while time_1 - time_2 < 0.15:
pub_cmd_vel.publish(vel_cmd)
time_1 = time.time()
Asked by Tima1995 on 2019-10-26 08:29:58 UTC
When running the code above, the following mistake occure for the node that publishes the action:
[WARN] [1572096179.848196, 54.768000]: Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes were received. Please check sender for additional details.
Asked by Tima1995 on 2019-10-26 08:41:05 UTC
Why don't you simply use a rate.sleep() as in the tutorials ? In your situation you continously publish messages during 0.15 sec and then wait to receive messages from cmd_vel_1.
Asked by Delb on 2019-10-31 04:27:14 UTC
The problem for me is, if I just publish once, without the time loop, the robot rarely gets the message. That's why I am constantly publish those over the given period of time. And I choose 0,15 sec because the sensor publishes data every 0.2 sec. So i am publishing for 0.15 sec, then I wait for next message, which is again published.
Asked by Tima1995 on 2019-10-31 04:49:57 UTC
the robot rarely gets the message
Have you tried sending latched messages ? (here is the doc)
Asked by Delb on 2019-10-31 04:54:16 UTC
So that the data is stored for some time? I thought that this one is for slow-changing data as siad in the documentation. But I will try that one out! Thanks for the answer
Asked by Tima1995 on 2019-10-31 08:05:26 UTC
Yes but as you said, your robot rarely gets the message so that could be a workaround to your issue.
Asked by Delb on 2019-10-31 08:15:38 UTC
Comments