Ask Your Question
0

Stopping teleop without shutting down SMACH state-machine

asked 2021-03-29 14:09:44 -0600

DanEB gravatar image

updated 2021-03-29 19:05:30 -0600

I'm building a mobile robot that is using SMACH for high-level task management. I'm starting of trying to give users the ability to chose between between exploration and teleoperation. I have no issues setting up the the state-machine, and and the transistions between states seems to work fine. Both teleop and exploration starts up after adding the user-input.

The issue I'm running into is when I want to stop the teleop_twist_keyboard-node, and transistion back to mode select state(teleop or exploration). The only way that seem to stop teleop is using CTRL-C, and that shuts down the whole program, including the state-machine. I thought about using threading, but teleop seems to allready be using it, so I'm not sure if that is the correct path to pursue.

Have anyone been able to find a way to stop the teleop_twist_keyboard-node and without shutting down the state-machine?

I will gladly post code, but I'm not sure what to post for this one.

UPDATE: I am now able to stop teleop without shutting down the state-machine with the help of subprocess.Popen. But now it won't let me control the robot from the terminal, and quits teleop preemptively. This the two main states I'm switching between:

class RoamMode(smach.State):
     def init(self):
          smach.State.init(self, outcomes= ['Exploraiton mode selected', 'Teleoperation mode selected', 'Wrong key'])

     def execute(self, userdata):
          user_input = input('Press T for Teleoperation mode, or press E for Exploration mode')

          if user_input == 'T':
              return 'Teleoperation mode selected'
           elif user_input =='E':
              return 'Exploraiton mode selected'
           else:
              return 'Wrong key'

class RoamTeleOp(smach.State):
    def init(self):
       smach.State.init(self, outcomes=['Teleoperation stopped'])

    def execute(self, userdata):
       tp_process = subprocess.Popen('python3 run_teleop.py', shell=True)
       if tp_process.poll() == None:
          tp_process.kill()
          rospy.sleep(10.)
          return 'Teleoperation stopped'

This is the other file I'm running with subprocess.Popen('python3 run_teleop.py', shell=True):

import rospy
import roslaunch

package = 'teleop_twist_keyboard'
executable = 'teleop_twist_keyboard.py'
node = roslaunch.core.Node(package, executable)
launch = roslaunch.scriptapi.ROSLaunch()
launch.start()
process = launch.launch(node)
rospy.spin()
edit retag flag offensive close merge delete

Comments

You can kill the teleop_node from your script.

Tahir M. gravatar image Tahir M.  ( 2021-03-29 15:16:55 -0600 )edit

You sugest using another thread to kill the node, while it's running?

EDIT: Still have not figured out a practical way of killing the node while it's running with rosnode.kill_nodes. Even tried with threading, but that made all sorts of mess.

DanEB gravatar image DanEB  ( 2021-03-29 15:26:57 -0600 )edit
1

Why do you want to shut down the teleop node? wouldn't it make more sense to extend it with a key to basically quit it, could be a service or a simple topic that you publish on additionally to the regular twist topic, that your state machine listens to and then transitions back out? You could additionally extend the teleop node to be triggered/enabled by the state machine

mgruhler gravatar image mgruhler  ( 2021-03-30 01:50:16 -0600 )edit

The way that teleop is implemented it made sense to me to stop the node, rather than running it through a topic with with some kind of key-interrupt. I am still unsure on how to actually make a topic that I can interrpt, since the communication is two-ways. I will closer into your suggestions. As a beginner to ROS I may be overcomplicating this matter. Your help is appreaciated.

DanEB gravatar image DanEB  ( 2021-03-30 13:02:24 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-04-03 13:49:47 -0600

Loy gravatar image

Would https://wiki.ros.org/twist_mux be of any help? I'd set that up with 2 inputs: from the exploration commands and one for the teleop/

That still does not directly cover stopping the exploration command generation in Smach, but you could implement that by subscribing to the teleop topic in the RoamMode state and then calling self.preempt() when there is a message received (and it's not already preempting etc)

edit flag offensive delete link more

Comments

This fixes it! I am a bit puzzled right now, because I am sure I checked twist_mux a few days ago, and it was not supported for Noetic?

Anyways, thank you!

DanEB gravatar image DanEB  ( 2021-04-03 14:04:13 -0600 )edit

might very well be that it just slipped in with the latest sync...

mgruhler gravatar image mgruhler  ( 2021-04-06 02:18:26 -0600 )edit

I just realized that it was yocs_cmd_vel_mux that I have been looking at.

DanEB gravatar image DanEB  ( 2021-04-06 03:02:37 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-03-29 14:09:44 -0600

Seen: 118 times

Last updated: Apr 03