Launch files from inside "master" node

asked 2020-01-02 14:00:38 -0500

aspai gravatar image

I am currently trying to create a master launch file which takes inputs from the users joystick. I currently a simple implementation of the roslaunch API but when I run as is I encounter the error: signal only works in main thread The intended operation is that a button press will launch a node (or set of nodes) in the background that can be terminated with the master node.

I have tried using system calls to force the process to launch another, but I encountered the same error. Is there a way to launch - launch files from a node efficiency and reliably?

#!/usr/bin/python
import roslaunch
import rospy
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from math import*
import sys
import thread
import subprocess
import os

calibration_request = False
request_complete = False

def launch_calibration():
    print("INSIDE launch calibration")
    uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
    roslaunch.configure_logging(uuid)

    cli_args = ['outdoor_waypoint_nav', 'sim_calibration.launch']
    roslaunch_file = roslaunch.rlutil.resolve_launch_arguments(cli_args)
    roslaunch_args = cli_args[2:]
    parent = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)

    try:
        parent.start()
        rospy.Duration(1)
    except rospy.ROSInterruptException:
        parent.shutdown()

# def launch_capture_waypoint():
def joy_CB(joy_msg):
    print("in callback")
    # return
    global calibration_request
    global request_complete

    if joy_msg.buttons[calibration_num] == True:
        print("X button pressed !")
        # return
        # launch_calibration()
        # bashCommand()
        launch_calibration()
        calibration_request = True
    else: 
        calibration_request = False
    if joy_msg.buttons[end_button_num] == True:
        request_complete = True

def main():
    global calibration_num
    global calibration_sym
    global end_button_num
    global end_button_sym
    global request_complete

    rospy.init_node('master', anonymous=False)

    calibration_num = rospy.get_param("/master/calibration_num")
    calibration_sym = rospy.get_param("/master/calibration_sym")

    end_button_num  = rospy.get_param("/master/end_button_num")
    end_button_sym = rospy.get_param("/master/end_button_sym")

    rate = rospy.Rate(1) # hz
    while not rospy.is_shutdown():
        print("looping")
        rospy.Subscriber("/joy_teleop/joy", Joy, callback=joy_CB)
        rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        sys.exit(0)
        # pass
edit retag flag offensive close merge delete

Comments

Is there a specific reason you want to launch and kill nodes from a master node? In my experience what typically is done is that all nodes are launched through roslaunch from the start and just sit there when they aren't used. The roslaunch API also isn't terribly well supported for Kinetic with a known bugfix that wasn't ported back to Kinetic.

achille gravatar image achille  ( 2020-01-05 15:49:15 -0500 )edit