class DragonTeleop(): def __init__(self): self.dm = DragonbotManager() self.dm.pose_off() self.exp_sub = rospy.Subscriber("/dragon_teleop_GUI/expressions", String, self.exp_callback) self.mot_sub = rospy.Subscriber("/dragon_teleop_GUI/motions", String, self.mot_callback) self.ph_sub = rospy.Subscriber("/dragon_teleop_GUI/phrases", String, self.ph_callback) self.pose_on_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_on", String, self.pos_on_cb) self.x_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_x", Int32, self.x_pos_cb) self.y_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_y", Int32, self.y_pos_cb) self.z_sub = rospy.Subscriber("/dragon_teleop_GUI/pose_z", Int32, self.z_pos_cb) self.look_on_sub = rospy.Subscriber("/dragon_teleop_GUI/lookat_on", String, self.look_on_cb) self.xl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_x", Int32, self.x_look_cb) self.yl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_y", Int32, self.y_look_cb) self.zl_sub = rospy.Subscriber("/dragon_teleop_GUI/look_z", Int32, self.z_look_cb) self.blink_sub = rospy.Subscriber("/dragon_teleop_GUI/blink", String, self.blink_cb) rospy.loginfo("Ready!") self.current_pose = [0,0,0] self.pose_on = False self.x_min = -2.3 self.x_max = 2.5 self.x_range = self.x_max - self.x_min self.y_min = -2.49 self.y_max = 3.4 self.y_range = self.y_max - self.y_min self.z_min = -2.0 self.z_max = 2.6 self.z_range = self.z_max - self.z_min self.v = 1 self.a = .05 self.current_look = [0,0,20] self.look_on = False self.x_lmin = -300 self.x_lmax = 300 self.x_lrange = self.x_lmax - self.x_lmin self.y_lmin = -300 self.y_lmax = 300 self.y_lrange = self.y_lmax - self.y_lmin self.z_lmin = 20 self.z_lmax = 400 self.z_lrange = self.z_lmax - self.z_lmin def exp_callback(self, data): rospy.loginfo("Got expression: " + data.data) self.dm.express(data.data) def mot_callback(self, data): rospy.loginfo("Got motion: " + data.data) self.dm.express(data.data) def ph_callback(self, data): rospy.loginfo("Got phrase: " + data.data) self.dm.say(data.data) def blink_cb(self, data): rospy.loginfo("Got blink command: " + data.data) if data.data == "once": self.dm.blink() elif data.data == "hold": self.dm.eye_close() elif data.data == "hold_off": self.dm.eye_open() def pos_on_cb(self, data): rospy.loginfo("Setting pose to: " + data.data) if data.data == "on": self.current_pose = [0,0,0] self.dm.pose(0,0,0) self.pose_on = True elif data.data == " off": self.pose_on = False self.dm.pose_off() def x_pos_cb(self, data): rospy.loginfo("Got x pose value: " + str(data.data)) if self.pose_on: self.current_pose[0] = self.x_min + (float(data.data)/100) * self.x_range rospy.loginfo("Setting pose to: " + str(self.current_pose)) self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a) def y_pos_cb(self, data): rospy.loginfo("Got y pose value: " + str(data.data)) if self.pose_on: self.current_pose[1] = self.y_min + (float(data.data)/100) * self.y_range rospy.loginfo("Setting pose to: " + str(self.current_pose)) self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a) def z_pos_cb(self, data): rospy.loginfo("Got z pose value: " + str(data.data)) if self.pose_on: self.current_pose[2] = self.z_min + (float(data.data)/100) * self.z_range rospy.loginfo("Setting pose to: " + str(self.current_pose)) self.dm.pose(self.current_pose[0], self.current_pose[1], self.current_pose[2], vel = self.v, acc = self.a) def look_on_cb(self, data): rospy.loginfo("Setting lookat to: " + data.data) if data.data == "on": self.current_look = [0,0,20] self.dm.lookat(0,0,20) self.look_on = True elif data.data == " off": self.look_on = False self.dm.lookat_off() def x_look_cb(self, data): rospy.loginfo("Got x lookat value: " + str(data.data)) if self.look_on: self.current_look[0] = self.x_lmin + (float(data.data)/100) * self.x_lrange rospy.loginfo("Setting lookat to: " + str(self.current_look)) self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2]) def y_look_cb(self, data): rospy.loginfo("Got y lookat value: " + str(data.data)) if self.look_on: self.current_look[1] = self.y_lmin + (float(data.data)/100) * self.y_lrange rospy.loginfo("Setting lookat to: " + str(self.current_look)) self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2]) def z_look_cb(self, data): rospy.loginfo("Got z lookat value: " + str(data.data)) if self.look_on: self.current_look[2] = self.z_lmin + (float(data.data)/100) * self.z_lrange rospy.loginfo("Setting lookat to: " + str(self.current_look)) self.dm.lookat(self.current_look[0], self.current_look[1], self.current_look[2])