service_handler.py

Go to the documentation of this file.

#!/usr/bin/env python3
 
import subprocess
import os
import fcntl
import sys
import rospy
 
from std_srvs.srv import Trigger, TriggerResponse
from vizanti.srv import GetNodeParameters, GetNodeParametersResponse
from vizanti.srv import LoadMap, LoadMapResponse, SaveMap, SaveMapResponse
from vizanti.srv import RecordRosbag, RecordRosbagResponse
from vizanti.srv import ManageNode, ListPackages, ListExecutables, ListExecutablesResponse, ListPackagesResponse
 
 
class ServiceHandler:
 
        def __init__(self):
                rospy.init_node('vizanti_service_handler')
 
                self.proc = None
                self.packages = self.get_packages()
 
                self.get_nodes_service = rospy.Service('vizanti/get_dynamic_reconfigure_nodes', Trigger,self. get_dynamic_reconfigure_nodes)
                self.get_node_parameters_service = rospy.Service('vizanti/get_node_parameters', GetNodeParameters, self.get_node_parameters)
 
                self.load_map_service = rospy.Service('vizanti/load_map', LoadMap, self.load_map)
                self.save_map_service = rospy.Service('vizanti/save_map', SaveMap, self.save_map)
 
                self.record_setup_service = rospy.Service('vizanti/bag/setup', RecordRosbag, self.recording_setup)
                self.record_status_service = rospy.Service('vizanti/bag/status', Trigger, self.recording_status)
 
                self.node_kill_service = rospy.Service('vizanti/node/kill', ManageNode, self.node_kill)
                self.node_start_service = rospy.Service('vizanti/node/start', ManageNode, self.node_start)
                self.node_info_service = rospy.Service('vizanti/node/info', ManageNode, self.node_info)
                self.node_info_service = rospy.Service('vizanti/roswtf', Trigger, self.roswtf)
 
                self.list_packages_service = rospy.Service('vizanti/list_packages', ListPackages ,self.list_packages_callback)
                self.list_executables_service = rospy.Service('vizanti/list_executables', ListExecutables ,self.list_executables_callback)
 
                rospy.loginfo("Service handler ready.")
 
        def get_packages(self):
                # Use rospack to get list of packages
                cmd = ["rospack", "list"]
                process = subprocess.Popen(cmd, stdout=subprocess.PIPE)
                output, error = process.communicate()
                # Process output
                lines = output.decode('utf-8').split('\n')
                packages = {line.split()[0]: line.split()[1] for line in lines if line}
                return packages
 
        def list_packages_callback(self, req):
                return ListPackagesResponse(self.packages.keys())
        
        def list_executables_callback(self, req):
                if req.package not in self.packages:
                        rospy.logerr("Package not found: " + req.package)
                        return ListExecutablesResponse([])
 
                path = self.packages[req.package]
                # Use find to get list of executables
                cmd_exec = ["find", path, "-type", "f", "!", "-name", "*.*", "-executable"]
                cmd_python_and_launch = ["find", path, "-type", "f", 
                                                                "(",
                                                                "-iname", "*.py", "-executable",
                                                                "-o",
                                                                "-iname", "*.launch",
                                                                ")"]
                                                                
                process_exec = subprocess.Popen(cmd_exec, stdout=subprocess.PIPE)
                process_python_and_launch = subprocess.Popen(cmd_python_and_launch, stdout=subprocess.PIPE)
                
                output_exec, error_exec = process_exec.communicate()
                output_python_and_launch, error_python_and_launch = process_python_and_launch.communicate()
                
                # Process output
                lines_exec = output_exec.decode('utf-8').split('\n')
                lines_python_and_launch = output_python_and_launch.decode('utf-8').split('\n')
                
                executables = [line.split("/")[-1] for line in lines_exec if line]
                python_and_launch_files = [line.split("/")[-1] for line in lines_python_and_launch if line]
                
                return ListExecutablesResponse(executables + python_and_launch_files)
 
        def node_kill(self, req):
                try:
                        subprocess.call(['rosnode', 'kill', req.node])
                        return {'success': True, 'message': f'Killed node {req.node}'}
                except Exception as e:
                        return {'success': False, 'message': str(e)}
 
        def node_start(self, req):
                try:
                        args = req.node.split(" ")
 
                        # Open /dev/null
                        devnull = open(os.devnull, 'w')
 
                        # Set up the process to ignore the SIGTERM signal
                        def preexec():
                                os.setpgrp()
                                sys.stdin = open(os.devnull, 'r')
                                sys.stdout = open(os.devnull, 'w')
                                sys.stderr = open(os.devnull, 'w')
 
                        subprocess.Popen(args, stdout=devnull, stderr=devnull, preexec_fn=preexec)
                        
                        return {'success': True, 'message': f'Started node {req.node}'}
                except Exception as e:
                        return {'success': False, 'message': str(e)}
                
        def node_info(self, req):
                try:
                        rosinfo = subprocess.check_output(["rosnode", "info", req.node]).decode('utf-8')
                        rosinfo = rosinfo.replace("--------------------------------------------------------------------------------","")
                        return {'success': True, 'message': rosinfo}
                except Exception as e:
                        return {'success': False, 'message': str(e)}
                
        def roswtf(self, req):
                try:
                        rosinfo = subprocess.check_output(["roswtf"]).decode('utf-8')
                        return {'success': True, 'message': rosinfo}
                except Exception as e:
                        return {'success': False, 'message': str(e)}
                
 
        def load_map(self, req):
                file_path = os.path.expanduser(req.file_path)
                topic = req.topic
                try:
                        process = subprocess.Popen(["rosrun", "map_server", "map_server", file_path, "map:=" + topic, "__name:=vizanti_map_server"], stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
                        flags = fcntl.fcntl(process.stdout, fcntl.F_GETFL)
                        fcntl.fcntl(process.stdout, fcntl.F_SETFL, flags | os.O_NONBLOCK)
 
                        # Wait for it to either fail or not
                        rospy.sleep(1)
 
                        # Check if the process is still running
                        if process.poll() is not None:
                                # Process terminated, read the error output
                                error_output = process.stdout.read().decode('utf-8')
                                return LoadMapResponse(success=False, message="Map server failed to load the map: " + error_output)
 
                        return LoadMapResponse(success=True, message="Map loaded successfully")
                except Exception as e:
                        return LoadMapResponse(success=False, message=str(e))
 
        def save_map(self, req):
                file_path = os.path.expanduser(req.file_path)
                topic = req.topic
                try:
                        process = subprocess.Popen(["rosrun", "map_server", "map_saver", "-f", file_path, "map:=" + topic, "__name:=vizanti_map_saver"], stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
                        flags = fcntl.fcntl(process.stdout, fcntl.F_GETFL)
                        fcntl.fcntl(process.stdout, fcntl.F_SETFL, flags | os.O_NONBLOCK)
 
                        while True:
                                # Check if the process is still running
                                if process.poll() is not None:
                                        break
 
                                while True:
                                        try:
                                                line = process.stdout.readline()
                                                if not line:
                                                        break
                                                
                                                if b"[ERROR]" in line:
                                                        process.terminate()
                                                        return SaveMapResponse(success=False, message="Map saver failed to save the map: " + line.decode('utf-8'))
                                        except IOError:
                                                break
 
                                # Sleep for a short period of time to avoid excessive CPU usage
                                rospy.sleep(0.2)
 
                        return SaveMapResponse(success=True, message="Map saved successfully")
                except Exception as e:
                        return SaveMapResponse(success=False, message=str(e))
 
        def get_dynamic_reconfigure_nodes(self, req):
                list_nodes_output = subprocess.check_output(["rosrun", "dynamic_reconfigure", "dynparam", "list"]).decode('utf-8')
                nodes_list = list_nodes_output.strip().split("\n")
 
                response = TriggerResponse()
                response.success = True
                response.message = "\n".join(nodes_list)
 
                return response
 
        def get_node_parameters(self, req):
                node_params = subprocess.check_output(["rosrun", "dynamic_reconfigure", "dynparam", "get", req.node]).decode('utf-8')
 
                response = GetNodeParametersResponse()
                response.parameters = node_params
 
                return response
 
        def recording_status(self, msg):
                response = TriggerResponse()
                response.success = self.proc is not None
 
                if response.success:
                        response.message = "Bag recording in progress..."
                else:
                        response.message = "Bag recorder idle."
 
                return response
 
 
        def recording_setup(self, req):
 
                response = RecordRosbagResponse()
 
                if req.start:
                        if self.proc is not None:
                                response.success = False
                                response.message = "Already recording, please stop the current recording first."
                        else:
                                command = ['rosbag', 'record', '-O']
 
                                # Expand and add the path to the command
                                expanded_path = os.path.expanduser(req.path)
                                command.append(expanded_path)
 
                                # Add the topics to the command
                                for topic in req.topics:
                                        command.append(topic)
 
                                # Use subprocess to start rosbag record in a new process
                                self.proc = subprocess.Popen(command)
 
                                response.success = True
                                response.message = "Started recording rosbag with PID " + str(self.proc.pid) +" to path "+ str(expanded_path)
                else:
                        if self.proc is None:
                                response.success = False
                                response.message = "No recording to stop."
                        else:
                                self.proc.terminate()
                                self.proc = None
 
                                response.success = True
                                response.message = "Stopped recording rosbag."
 
                return response
 
 
handler = ServiceHandler()
rospy.spin()