vrep_environment_publisher.py 文件源码

def __init__(self):
        self.joy_pub = rospy.Publisher('sensors/joystick/1', Joy, queue_size=1)
        self.joy_pub2 = rospy.Publisher('sensors/joystick/2', Joy, queue_size=1)
        self.ball_pub = rospy.Publisher('environment/ball', CircularState, queue_size=1)
        self.light_pub = rospy.Publisher('environment/light', UInt8, queue_size=1)
        self.sound_pub = rospy.Publisher('environment/sound', Float32, queue_size=1)

        self.rospack = RosPack()
        with open(join(self.rospack.get_path('apex_playground'), 'config', 'ergo.json')) as f:
            self.ergo_params = json.load(f)
        with open(join(self.rospack.get_path('apex_playground'), 'config', 'environment.json')) as f:
            self.env_params = json.load(f)

        self.rate = rospy.Rate(self.ergo_params['publish_rate'])
        vrep_port = rospy.get_param('vrep/environment_port', 29997)
        self.simulation_id = vrep.simxStart('127.0.0.1', vrep_port, True, False, 5000, 5)

        # Object names in V-Rep
        self.joystick_left_joints = ['Joystick_2_Axis_2', 'Joystick_2_Axis_1']
        self.joystick_right_joints = ['Joystick_1_Axis_2', 'Joystick_1_Axis_1']
        self.ball_name = 'TennisBall'
        self.arena_name = 'Arena'

        if self.ergo_params["control_joystick_id"] != 2:
            useless_joy = self.joystick_left_joints
            self.joystick_left_joints = self.joystick_right_joints
            self.joystick_right_joints = useless_joy

        self.joints = JointTracker(self.joystick_left_joints + self.joystick_right_joints, self.simulation_id)
        self.objects = ObjectTracker([self.ball_name, self.arena_name], self.simulation_id)
        self.conversions = EnvironmentConversions()