通过python启动ros功能节点

在机器人使用过程中，经常会与第三方软件交互来控制机器人相应功能的启动。本文利用python通过多进程的方式实现了对ros相应节点的启动关闭等功能。以启动关闭slam、修改配置文件、设置目标点为例，具体代码如下：

#!/usr/bin/python2.7

import json
import os
import subprocess
from multiprocessing import Process
# import ruamel.yaml
import yaml
import rospy
import tf
import actionlib
from actionlib_msgs.msg import * 
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
import math
import time
from std_msgs.msg import Bool
import psutil


def sub_process(shell_name):
    print("first time get pid ")
    print(os.getpid())
    os.system("./{shell_name}.sh")
    print("second time get pid ")
    print(os.getpid())


def operate_yaml(path):
    # yaml = ruamel.yaml.YAML()
    with open(path, encoding='utf-8') as f:
        content = yaml.load(f)
    return content


def write_yaml(path, content):
    # yaml = ruamel.yaml.YAML()
    with open(path,"w", encoding='utf-8') as f:
        yaml.dump(content, f)



class Execute:
    cmd_instruction = '''#!/bin/bash
source /home/lll/nav_ws/devel/setup.bash
'''


    def __init__(self, resp):
        self.resp = json.loads(resp)


    def action(self, target):
        f = getattr(self, target)
        execute_message = f()
        return json.dumps(execute_message)


    def shell_action(self, current_instruction, shell_name):
        cmd_instruction_in = self.cmd_instruction + current_instruction
        with open("{shell_name}.sh", 'w') as fp:
            fp.write(cmd_instruction_in)
        try:
            os.chmod("{shell_name}.sh", 0o774)
            p = Process(target=sub_process, args=(shell_name,))
            p.start()
            return 1
        except:
            return 0


    def set_targetpoint(self):
        print("set_targetpoint")
        return_message = {
                "msg": {
                    "error_code": 0,
                    "error_msg": "",
                    "result": 1
                }
            }
        goal_x = float(self.resp['msg']['x'])
        goal_y = float(self.resp['msg']['y'])
        goal_z = float(self.resp['msg']['z'])
        goal_theta = float(self.resp['msg']['theta'])
        # goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
        #                 'SUCCEEDED', 'ABORTED', 'REJECTED',
        #                 'PREEMPTING', 'RECALLING', 'RECALLED',
        #                 'LOST']
        current_instruction = '\nrosservice call move_base/clear_costmaps "{}"'
        shell_name = 'clear_costmaps'
        shell_result = self.shell_action(current_instruction, shell_name)
        if shell_result == 1:
            rospy.loginfo("clear_costmaps succeed")
        else:
            rospy.loginfo("clear_costmaps failed")
        move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        rospy.loginfo("Waiting for move_base action server")
        move_base.wait_for_server(rospy.Duration(60))
        rospy.loginfo("connected to move base server")

        goal = MoveBaseGoal()
        quaternion = tf.transformations.quaternion_from_euler(0,0,goal_theta,axes='sxyz')
        goal.target_pose.pose = Pose(Point(goal_x, goal_y, goal_z), Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]))
        # goal.target_pose.pose = Pose(Point(goal_x, goal_y, goal_z), Quaternion(0,0,0,1))
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now()
        rospy.loginfo("Going to %.4f, %.4f, %.4f" %(goal_x, goal_y, goal_theta))
        move_base.send_goal(goal)
        finished_within_time = move_base.wait_for_result(rospy.Duration(600))
        print("finished_within_time:",finished_within_time)

        if not finished_within_time:
            move_base.cancel_goal()
            return_message['msg']['result'] = 0
            return_message['msg']['error_msg'] = "didn't reach goal within time"
            return return_message
        else:
            state = move_base.get_state()
            print("state:",state)
            if state == GoalStatus.SUCCEEDED:
                rospy.loginfo("Goal succeed!")
                return_message['msg']['result'] = 1
                return return_message
            else:
                return_message['msg']['result'] = 0
                return_message['msg']['error_msg'] = "nav failed"
                return return_message



    def set_navigation_parameter(self):
        print("set_navigation_parameter")
        return_message = {
                "msg": {
                    "error_code": 0,
                    "error_msg": "",
                    "result": 1
                }
        }

        try:
            path ='/home/lll/nav_ws/src/navigation2D/param/teb_local_planner_params.yaml'
            content = operate_yaml(path)
            content['TebLocalPlannerROS']['max_vel_x']=self.resp['msg']['move_speed_v']
            content['TebLocalPlannerROS']['max_vel_theta']=self.resp['msg']['move_speed_w']
            content['TebLocalPlannerROS']['xy_goal_tolerance']=self.resp['msg']['xy_goal_tolerance']
            write_yaml(path,content)
            return return_message
        except:
            return_message['msg']['error_code'] = 997
            return_message["msg"]['error_msg'] = 'failed to set_navigation_parameter'
            return_message["msg"]['result'] = 0
            return return_message


    def slam(self):
        print("start to slam")
        ctrl_flag = int(self.resp['msg']['action'])
        return_message = {
                "msg": {
                    "error_code": 0,
                    "error_msg": "",
                    "result": 1
                }
            }
        if 1 == ctrl_flag: 
            current_instruction = '\ncd /home/lll/slam; source devel/setup.bash; roslaunch slam run.launch'
            shell_name = 'roslaunch_slam'
            shell_result = self.shell_action(current_instruction, shell_name)
            count = 10
            while count > 0:
                status = subprocess.call("ps -ef |grep mainSystem | grep -v grep", shell=True)
                if status == 0:
                    print("slam start!")
                    return return_message
                rospy.sleep(1)
                count = count - 1    

            return_message['msg']['error_code'] = 997
            return_message["msg"]['error_msg'] = 'failed to start slam'
            return_message["msg"]['result'] = 0
            return return_message

        else:
            status = subprocess.call("ps -ef | grep run.launch | grep -v grep |awk '{print $2}' | xargs kill", shell=True)
            if status == 0:
                return return_message
            else:
                return_message['msg']['error_code'] = 997
                return_message["msg"]['error_msg'] = 'failed to stop slam'
                return_message["msg"]['result'] = 0
                return return_message               


    def is_run(self, name):
        return_message = {
                "msg": {
                    "error_code": 0,
                    "error_msg": "",
                    "result": 2
                }
            }
        try:
            status = subprocess.call("ps -ef |grep " + name +" | grep -v grep", shell=True)
            print("status:", status)
            if status == 0:
                return_message['msg']['result'] = 1
                print(" " + name +" is on")
                return return_message
            else:
                return_message['msg']['result'] = 0
                print(" " + name +" is off")
                return return_message
        except:
            return_message["msg"]['error_msg'] = 'failed to get state'
            return return_message