def run(self):
self.start_simulation()
rospy.Service('vrep/reset_ball', Empty, self.cb_reset_ball)
try:
self.reset_ball()
while not rospy.is_shutdown():
self.joints.update()
self.objects.update()
joints = self.joints.get()
def map_joy(x):
h_joy = 2.
return min(max(h_joy*sin(x), -1), 1)
x = map_joy(joints[self.joystick_left_joints[0]]['position'])
y = map_joy(joints[self.joystick_left_joints[1]]['position'])
# Publishers
self.publish_joy(x, y, self.joy_pub)
x = map_joy(joints[self.joystick_right_joints[0]]['position'])
y = map_joy(joints[self.joystick_right_joints[1]]['position'])
self.publish_joy(x, y, self.joy_pub2)
objects = self.objects.get()
if 'position' in objects[self.ball_name] and 'position' in objects[self.arena_name]:
ring_radius = self.env_params['tracking']['arena']['radius'] / self.env_params['tracking']['ring_divider'] # There is no visual tracking so the true arena diameter is assumed
ball_state = self.conversions.get_state(objects[self.ball_name]['position'], # Although returned by V-Rep, dimension "z" is ignored
objects[self.arena_name]['position'],
ring_radius)
self.ball_pub.publish(ball_state)
color = self.conversions.ball_to_color(ball_state)
self.light_pub.publish(UInt8(color))
sound = self.conversions.ball_to_sound(ball_state)
self.sound_pub.publish(Float32(sound))
distance = norm(array(objects[self.ball_name]['position']) - array(objects[self.arena_name]['position']))
if distance > 0.25:
self.reset_ball()
self.rate.sleep()
finally:
self.stop_simulation()
sleep(0.5)
vrep.simxFinish(self.simulation_id)
评论列表
文章目录
