def callBack(msg):
pose_vel = PoseVelocity()
e = tft.euler_from_quaternion([
msg.glovesData[0].wristIMU.x, msg.glovesData[0].wristIMU.y,
msg.glovesData[0].wristIMU.z, msg.glovesData[0].wristIMU.w
])
print(e)
x = e[0]
y = e[1]
z = e[2]
# 旋转 just for left hand, horizon is zero；
if y > 1.2 or y < -0.5:
if y > 1.2: #right
pose_vel.twist_angular_z = -0.1
elif y < -0.5: #left
pose_vel.twist_angular_z = 0.1
elif z > 1 or z < -1:
if z > 1: # up
pose_vel.twist_angular_y = -0.1
elif z < -1:
pose_vel.twist_angular_y = 0.1
elif x > 1 or x < -1:
if x > 1: # ni
pose_vel.twist_angular_z = -0.1
elif x < -1: # shun
pose_vel.twist_angular_z = 0.1
# 平移
if msg.glovesData[0].fingersFlex[1].Joint1Stretch > 0.3:
pose_vel.twist_linear_z = -0.1
if msg.glovesData[0].fingersFlex[1].Joint1Stretch < -0.3:
pose_vel.twist_linear_z = 0.1
if msg.glovesData[0].fingersFlex[2].Joint1Stretch > 0.3:
pose_vel.twist_linear_y = -0.1
if msg.glovesData[0].fingersFlex[2].Joint1Stretch < -0.3:
pose_vel.twist_linear_y = 0.1
if msg.glovesData[0].fingersFlex[3].Joint1Stretch > 0.3:
pose_vel.twist_linear_x = -0.1
if msg.glovesData[0].fingersFlex[3].Joint1Stretch < -0.3:
pose_vel.twist_linear_x = 0.1
# CURRENT_VELOCITY[3] = 1 * dr #x: end effector self rotate,positive is shun
# CURRENT_VELOCITY[4] = 1 * dp #y: up and down rotate (positive is down)
# CURRENT_VELOCITY[5] = 1 * dyaw #z: left and right rotate,positive is left
pub.publish(pose_vel)
def run(self):
rate = rospy.Rate(100)
while not rospy.is_shutdown():
vel = PoseVelocity()
vel.twist_linear_x = self.x
vel.twist_linear_y = self.y
vel.twist_linear_z = self.z
vel.twist_angular_x = 0.0
vel.twist_angular_y = 0.0
vel.twist_angular_z = 0.0
self.pub.publish(vel)
rate.sleep()
def publishArmLinControl(self):
pose_vel_msg = PoseVelocity()
pose_vel_msg.twist_linear_x = self.joy.axes[
1] * self.v_lin_gain # Up/Down Axis stick left
pose_vel_msg.twist_linear_y = self.joy.axes[
0] * self.v_lin_gain # Left/Right Axis stick left
pose_vel_msg.twist_linear_z = self.joy.axes[
3] * self.v_lin_gain # Up/Down Axis stick right
pose_vel_msg.twist_angular_x = 0
pose_vel_msg.twist_angular_y = 0
pose_vel_msg.twist_angular_z = 0
self.pub_pose_vel_cmd.publish(pose_vel_msg)
def publishArmAngControl(self):
pose_vel_msg = PoseVelocity()
pose_vel_msg.twist_linear_x = 0
pose_vel_msg.twist_linear_y = 0
pose_vel_msg.twist_linear_z = 0
pose_vel_msg.twist_angular_x = self.joy.axes[
1] * self.w_ang_gain # Up/Down Axis stick left
if self.joy.buttons[1] == 0: # if lip is not pressed
pose_vel_msg.twist_angular_y = self.joy.axes[
0] * self.w_ang_gain # Left/Right Axis stick left
elif self.joy.buttons[1] == 1: # if lip is pressed
pose_vel_msg.twist_angular_z = self.joy.axes[
0] * -self.w_ang_gain # Left/Right Axis stick right
self.pub_pose_vel_cmd.publish(pose_vel_msg)
def publishArmLinControl(self):
pose_vel_msg = PoseVelocity()
pose_vel_msg.twist_linear_x = self.joy.axes[
1] * self.v_lin_gain # Left\Right
if self.joy.buttons[1] == 0: # if lip is not pressed
pose_vel_msg.twist_linear_y = self.joy.axes[
0] * self.v_lin_gain # Forward\Backward
elif self.joy.buttons[1] == 1: # if lip is pressed
pose_vel_msg.twist_linear_z = self.joy.axes[
0] * self.v_lin_gain # Up/Down Axis stick right
pose_vel_msg.twist_angular_x = 0
pose_vel_msg.twist_angular_y = 0
pose_vel_msg.twist_angular_z = 0
self.pub_pose_vel_cmd.publish(pose_vel_msg)