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

Interrupting teleop from custom package

asked 2018-12-15 08:42:33 -0500

sisko gravatar image

updated 2018-12-16 01:50:25 -0500

jayess gravatar image

Hi all.

Please I need some help with code I'm writing to interrupt the teleop node on my Turtlebot3. I have the following code:

#! /usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

#defines a callback
def callback(msg):
    # while not rospy.is_shutdown():
    print('==================================')

    # REAR
    print('REAR [0]')
    print msg.ranges[0]

    # RIGHT
    print('RIGHT [90]')
    print msg.ranges[90]

    # FRONT
    print('FRONT [180]')
    print msg.ranges[180]
    if msg.ranges[180] == 0:
        print "STOPPING FRONT MOTION"
        move = Twist()
        move.linear.x = 0.0
        move.angular.z = 0.0
        pub.publish(move)

    # LEFT
    print('LEFT [270]')
    print msg.ranges[270]


if __name__ == '__main__':
    try:
        rospy.init_node('obstacle_avoidance')
        pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        rospy.Subscriber('/scan', LaserScan, callback)
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

In the section of code commented # FRONT, the robot is in motion using the teleoperation package and I am attempting to stop the robot cold in it's tracks. However, what happens is the wheel splutter i:e stop and continues in very quick succession.

Can anyone help point out my error, please?

edit retag flag offensive close merge delete

Comments

Can anyone help point out my error, please?

you're not blocking the incoming msg stream that tells the robot to move forward.

So the robot receives 0 and normal msgs in rapid succession, leading to the stuttering you experience.

gvdhoorn gravatar image gvdhoorn  ( 2018-12-15 09:28:53 -0500 )edit

Thanks for your input @gvdhoorn but how do I block the msg stream?

sisko gravatar image sisko  ( 2018-12-15 11:33:12 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-12-15 18:43:48 -0500

billy gravatar image

Option 1:
See this link for how to multiplex the cmd_vel topic instead of having two nodes sending conflicting commands to the robot.

http://wiki.ros.org/cmd_vel_mux

If you see an obstacle, have your node take over control of the multiplexer and stop the robot. Of course then you need to figure out how to give control back to telop.

Option 2:
Remap the output of telop to go through your node so only your node publishes on the cmd_vel topic. If you see an obstacle, modify the telop output running through your node to stop the robot.

edit flag offensive delete link more

Comments

@sisko: I was going to suggest looking into cmd_vel_mux as well (but the newer version: yocs_cmd_vel_mux).

gvdhoorn gravatar image gvdhoorn  ( 2018-12-16 07:56:24 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-12-15 08:42:33 -0500

Seen: 405 times

Last updated: Dec 15 '18