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

Start launchfile from service

asked 2017-04-25 05:39:06 -0500

follesoe gravatar image

I'm looking into using the roslaunch python API to start/stop a given launch file based on service calls. The roslaunch API is fairly straight forward to work with, but I've run into a threading issue. When calling roslaunch.parent.ROSLaunchParentfrom my Service Handler function, I get the following error: Error processing request: signal only works in main thread.

This makes sense, as the documentation clearly states that ROSLaunchParent must be called from the main thread, while the service handler function is clearly running in its own thread.

So my question is: is there an easy way to switch context/execute the ROSLaunchParent call in the main thread of the node hosting the service?

The node looks like this:

from rosbag_logging.srv import *
import rospy
import roslaunch

class LoggingNode:
    def __init__(self):
        self.launch = None
        self.start_service = rospy.Service('rosbag_logging/start', StartLogging, self._srv_start_logging)
        self.stop_service = rospy.Service('rosbag_logging/stop', StopLogging, self._srv_stop_logging)
        self.status_service = rospy.Service('rosbag_logging/status', GetLoggingStatus, self._srv_status_logging)

    def _srv_start_logging(self, req):
        if self.launch == None:
            uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
            roslaunch.configure_logging(uuid)
            self.launch = roslaunch.parent.ROSLaunchParent(uuid, ['rosbag_logging.launch'])
            self.launch.start()
        return EmptyResponse()

    def _srv_stop_logging(self, req):
        if self.launch != None:
            self.launch.shutdown()
            self.launch = None
        return EmptyResponse()

    def _srv_status_logging(self, req):
        is_running = self.launch != None
        return GetLoggingStatusResponse(is_running)

if __name__ == "__main__":
    rospy.init_node('rosbag_logging')
    logging_node = LoggingNode()
    rospy.spin()
edit retag flag offensive close merge delete

Comments

1

The ROS capabilities provides a set of services for discovering, starting and stopping launch files using ROS services. Maybe you can use that instead of writing your own version?

ahendrix gravatar image ahendrix  ( 2017-04-25 12:59:17 -0500 )edit

@follesoe Did you figure out a bespoke solution or a solution using capabilities?

surfertas gravatar image surfertas  ( 2019-03-23 10:34:40 -0500 )edit

I would like to know it too

rezenders gravatar image rezenders  ( 2020-12-15 11:31:31 -0500 )edit

Any developments on this topic? I face the same problem - I cannot launch a node from the ros server.

mac137 gravatar image mac137  ( 2021-02-09 10:12:59 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2020-12-27 16:33:06 -0500

Pepis gravatar image

I think a straightforward way is to use a Queue object that holds a boolean value. The service callback function can set or reset the bit in the queue and then the main thread can start or stop the launchfile using the roslaunch API. You could adapt the following pseudocode to your needs

from Queue import Queue
import rospy
from your_package.srv import StartPackage, StopPackage


def set_queue_item(queue):
    queue.get()
    queue.put(True)


def reset_queue_item(queue):
    queue.get()
    queue.put(False)


def read_from_queue(queue):
    current_item = queue.get()
    queue.put(current_item)
    return current_item


def start_package(req, queue):
    # callback method the StartPackage service
    # the req parameter contains the message received from the client
    set_queue_item(queue)


def stop_package(req, queue):
    # callback method the StopPackage service
    # the req parameter contains the message received from the client
    reset_queue_item(queue)


if __name__ == '__main__':
    queue = Queue(1)    #create a queue of just 1 element
    queue.put(False)
    package_launched = False
    rospy.init_node('your_node')
    start_package_service = rospy.Service('start_package', StartPackage, lambda req: start_package(req, queue))
    stop_package_service = rospy.Service('stop_package', StopPackage, lambda req: stop_package(req, queue))
    while not rospy.is_shutdown():
        if read_from_queue(queue) and not package_launched:
            launch('yourfile.launch')  # implement your custom launch method using the roslaunch API
            package_launched = True
        if not read_from_queue(queue) and package_launched:
            stop('yourfile.launch')    # implement your custom stop method using the roslaunch API
            package_launched = False
edit flag offensive delete link more
0

answered 2019-08-01 05:57:52 -0500

felix_v gravatar image

Not perfect but one way round this problem is to trigger launch.start() in the main thread by passing the variable self.launch generated in _srv_start_logging and any other required information to the main thread using global variables. Then in the main thread have a loop that performs the start/stop actions.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2017-04-25 05:39:06 -0500

Seen: 2,026 times

Last updated: Apr 25 '17