как вызвать rospy.init_node () дважды - PullRequest
Новая
       13

как вызвать rospy.init_node () дважды

2 голосов
/ 05 апреля 2019

Я пытаюсь вызвать один узел для выполнения одного действия, а затем перейти к следующему действию.получить ошибку: поднять rospy.exceptions.ROSException ("rospy.init_node () уже был вызван с другими аргументами:" + str (_init_node_args))

как я могу остановить первый узел, чтобы продолжитьследующий?

Я попытался использовать rospy.signal_shutdown ('Quit'), но программа дырок остановилась, и не только нужный мне узел. 

#!/usr/bin/env python

import os
import sys
import rospy
import baxter_interface
import time

import argparse

from geometry_msgs.msg import (    PoseStamped,    Pose,    Point,        Quaternion,)
from std_msgs.msg import Header
from baxter_core_msgs.srv import (    SolvePositionIK,        SolvePositionIKRequest,)

def ik_test(limb):
    rospy.init_node("rsdk_ik_service_client", disable_signals=True)
    ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest()
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')

    poses = {
        'left': PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                    x=0.657579481614,
                    y=0.851981417433,
                    z=0.0388352386502,
                ),
                orientation=Quaternion(
                    x=-0.366894936773,
                    y=0.885980397775,
                    z=0.108155782462,
                    w=0.162162481772,
                ),
            ),
        ),
        'right': PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point(
                x=0.692002,
                y=-0.360751,
                z=-0.05133,
                ),
                orientation=Quaternion(
                    x=-0.105882425658, 
                    y=0.9364525476, 
                    z=-0.0241838726041,
                    w=0.333557608721,
                ),
            ),
        ),
    }

    ikreq.pose_stamp.append(poses[limb])

    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return 1

    if (resp.isValid[0]):
        print("SUCCESS - Valid Joint Solution Found:")
        # Format solution into Limb API-compatible dictionary
        limb_joints = dict(zip(resp.joints[0].name,         resp.joints[0].position))
        print limb_joints
    else:
        print("INVALID POSE - No Valid Joint Solution Found.")

    return limb_joints

def open_gripper():
    rospy.init_node('Hello_Baxter')

    limb = baxter_interface.Limb('right')
    from baxter_interface import Gripper
    right_gripper = Gripper('right')
    joints_names=        ['right_e0','right_e1','right_s0','right_s1','right_w0','right_w1','right_w2']

    r=rospy.Rate(10) #10Hz
    r.sleep()

    baxter_interface.gripper.Gripper('right').open() #Open right gripper
    return 0


def main():

    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                 description=main.__doc__)
    parser.add_argument(
        '-l', '--limb', choices=['left', 'right'], required=True,
        help="the limb to test"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    limb_joints=ik_test(args.limb)
    limb = baxter_interface.Limb('right')
    limb.move_to_joint_positions(limb_joints)

    open_gripper()

    return 0 #ik_test(args.limb)


if __name__ == '__main__':
    sys.exit(main())

Я ожидал первый узелчтобы выключить и другой, чтобы инициировать, но я получаю ошибку выше.

1 Ответ

0 голосов
/ 05 апреля 2019

Вы, похоже, путаете способ использования узлов ROS и сценариев Python. Ваш скрипт на python должен быть одним узлом. Внутри этого узла вы можете создавать и уничтожать разные темы, сервисы и действия в разное время, но вы можете создать только один узел один раз.

Вы должны инициировать сингл в начале своей основной функции, затем вы можете создавать различные службы и действия, которые вам нужны, когда они вам нужны.

Надеюсь, это поможет.

...