def create_jaco_pose_vel_cmd(self, cmd_data):
pose_cmd = JacoCartesianVelocityCmd()
pose_cmd.x = cmd_data[0]
pose_cmd.y = cmd_data[1]
pose_cmd.z = cmd_data[2]
pose_cmd.theta_x = cmd_data[3]
pose_cmd.theta_y = cmd_data[4]
pose_cmd.theta_z = cmd_data[5]
return pose_cmd
def subscribe_force_callback_left(data):
global lf
lf = JacoCartesianVelocityCmd()
lf.x = data.wrench.force.x
lf.y = data.wrench.force.y
lf.z = data.wrench.force.z
lf.theta_x = data.wrench.torque.x
lf.theta_y = data.wrench.torque.y
lf.theta_z = data.wrench.torque.z
def subscribe_force_callback_right(data):
# print("========")
# print(data)
global rf
rf = JacoCartesianVelocityCmd()
rf.x = data.wrench.force.x
rf.y = data.wrench.force.y
rf.z = data.wrench.force.z
rf.theta_x = data.wrench.torque.x
rf.theta_y = data.wrench.torque.y
rf.theta_z = data.wrench.torque.z
def move(self, arm, le_ri=0, up_dn=0):
raise NotImplemented("try move_relate")
'''
le_ri: [-1, 1]
up_dn: [-1, 1]
'''
assert arm in ['left', 'right']
vel_x = 0.15 * le_ri
vel_y = -0.15 * up_dn
vel_z = self.insert_speed
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
arm_cmd.x = vel_x
arm_cmd.y = vel_y
arm_cmd.z = vel_z
r = rospy.Rate(1000)
if arm == 'left':
timeout = time.time() + 0.05
while not rospy.is_shutdown():
self.arm_pub[0].publish(arm_cmd)
if time.time() > timeout:
break
r.sleep()
print('run')
if arm == 'right':
timeout = time.time() + 0.05
while not rospy.is_shutdown():
self.arm_pub[1].publish(arm_cmd)
if time.time() > timeout:
break
r.sleep()
print('run')
def _movo_teleop(self, joyMessage):
self._parse_joy_input(joyMessage)
dt = rospy.get_time() - self.last_joy
self.last_joy = rospy.get_time()
if self.button_state['MAP_BASE_A']:
self.teleop_control_mode_msg.data = "base_ctl"
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['MAP_rARM_B']:
self.teleop_control_mode_msg.data = "arm_ctl_right"
self.run_arm_ctl_right = True
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['MAP_lARM_X']:
self.teleop_control_mode_msg.data = "arm_ctl_left"
self.run_arm_ctl_right = False
self.run_arm_ctl_left = True
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['MAP_HEAD_Y']:
self.teleop_control_mode_msg.data = "pan_tilt_ctl"
# only mux topic when run_pan_tilt_ctl is changed from False to true
if self.run_pan_tilt_ctl == False:
self.head_cmd_current_source_index = (
self.head_cmd_current_source_index + 1) % len(
self.head_cmd_source)
rospy.wait_for_service('/head_cmd_mux/select')
switch_request = MuxSelectRequest()
switch_request.topic = '/movo/head/' + self.head_cmd_source[
self.head_cmd_current_source_index] + '/cmd'
self.head_cmd_srv.call(switch_request)
self.run_arm_ctl = False
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = True
self._init_pan_tilt = True
if self.button_state['MAP_eSTOP_power']:
self.teleop_control_mode_msg.data = "estop"
self.run_arm_ctl = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
self.arm_pub[0].publish(arm_cmd)
self.arm_pub[1].publish(arm_cmd)
if self.button_state['MAP_HOME_start']:
self.teleop_control_mode_msg.data = "home_arms"
if not (self.homing_sent):
tmp = Bool()
tmp.data = True
self.home_arm_pub.publish(tmp)
self.homing_sent = True
self.homing_start_time = rospy.get_time()
if (self.homing_sent):
if ((rospy.get_time() - self.homing_start_time) >
10.0) and not self.button_state['MAP_HOME_start']:
self.homing_sent = False
if self.button_state['MAP_eSTOP_power']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = DISABLE_REQUEST
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
"""
publish current control mode to inform other nodes
"""
self.teleop_control_mode_pub.publish(self.teleop_control_mode_msg)
if self.run_arm_ctl_right or self.run_arm_ctl_left:
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
gripper_cmd = GripperCmd()
kg_cmd = Float32()
if self.run_arm_ctl_right:
arm_idx = 0
else:
arm_idx = 1
if self.axis_value['MAP_TRIGGER_RT'] == -1.0:
if 0 == arm_idx:
arm_cmd.x = self.axis_value[
'MAP_TWIST_LR_stickleft'] * -self.arm_vel_lim
else:
arm_cmd.x = self.axis_value[
'MAP_TWIST_LR_stickleft'] * -self.arm_vel_lim
arm_cmd.z = self.axis_value[
'MAP_TWIST_UD_stickleft'] * self.arm_vel_lim
if not self.button_state['MAP_TORS_back_']:
arm_cmd.y = self.axis_value[
'MAP_TWIST_UD_stickright'] * -self.arm_vel_lim
else:
dt = rospy.get_time() - self.last_arm_update
self.lincmd.desired_position_m += (
self.axis_value['MAP_TWIST_UD_stickright'] *
self.lin_act_vel_lim) * dt
if (self.lincmd.desired_position_m > 0.464312):
self.lincmd.desired_position_m = 0.464312
elif self.lincmd.desired_position_m < 0.0:
self.lincmd.desired_position_m = 0.0
self.lincmd.header.stamp = rospy.get_rostime()
self.lincmd.header.frame_id = ''
self.linpub.publish(self.lincmd)
self.lincmd.header.seq += 1
self.last_arm_update = rospy.get_time()
arm_cmd.theta_y = self.axis_value['MAP_CROSS_LR'] * 100.0
arm_cmd.theta_x = self.axis_value['MAP_CROSS_UD'] * 100.0
if self.button_state['MAP_lWRIST_LB']:
arm_cmd.theta_z = 100.0
elif self.button_state['MAP_rWRIST_RB']:
arm_cmd.theta_z = -100.0
if self.button_state['MAP_stby_rstick']:
kg_cmd.data = 5000.0
elif self.button_state['MAP_TRACT_lstick']:
kg_cmd.data = -5000.0
else:
kg_cmd.data = 0.0
gripper_val = (self.axis_value['MAP_TRIGGER_LT'] + 1.0) / 2.0
if abs(self._last_gripper_val - gripper_val) > 0.05:
gripper_cmd.position = gripper_val * 0.085
gripper_cmd.speed = 0.05
gripper_cmd.force = 100.0
self.gripper_pub[arm_idx].publish(gripper_cmd)
self._last_gripper_val = gripper_val
self.arm_pub[arm_idx].publish(arm_cmd)
self.kgripper_pub[arm_idx].publish(kg_cmd)
elif self.run_pan_tilt_ctl:
if self.head_cmd_source[
self.head_cmd_current_source_index] == 'teleop':
vel_cmd = [0.0, 0.0]
if self.axis_value['MAP_TRIGGER_RT'] == -1.0:
vel_cmd[0] = -self.axis_value[
'MAP_TWIST_LR_stickleft'] * self.pt_vel_lim
vel_cmd[1] = -self.axis_value[
'MAP_TWIST_UD_stickright'] * self.pt_vel_lim
self.pt_cmds.pan_cmd.pos_rad += vel_cmd[0] * dt
self.pt_cmds.tilt_cmd.pos_rad += vel_cmd[1] * dt
self.pt_cmds.pan_cmd.pos_rad = limit_f(
self.pt_cmds.pan_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.tilt_cmd.pos_rad = limit_f(
self.pt_cmds.tilt_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.pan_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.pt_cmds.tilt_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.p_pub.publish(self.pt_cmds)
else:
if self.button_state['MAP_eSTOP_power']:
#handled at top of function
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
elif self.button_state['MAP_stby_rstick']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = STANDBY_REQUEST
elif self.button_state['MAP_TRACT_lstick']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = TRACTOR_REQUEST
else:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
if ('GENERAL_PURPOSE_CMD_NONE' != self.cfg_cmd.gp_cmd):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = True
elif (True == self.send_cmd_none):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = False
elif (False == self.send_cmd_none):
if self.axis_value['MAP_TRIGGER_RT'] == -1.0:
self.motion_cmd.linear.x = (
self.axis_value['MAP_TWIST_UD_stickleft'] *
self.x_vel_limit_mps)
self.motion_cmd.linear.y = (
self.axis_value['MAP_TWIST_LR_stickleft'] *
self.y_vel_limit_mps)
self.motion_cmd.angular.z = (
self.axis_value['MAP_TWIST_LR_stickright'] *
self.yaw_rate_limit_rps)
self.last_motion_command_time = rospy.get_time()
else:
self.motion_cmd.linear.x = 0.0
self.motion_cmd.linear.y = 0.0
self.motion_cmd.angular.z = 0.0
if (dt >= 0.01):
self.limited_cmd.linear.x = slew_limit(
self.motion_cmd.linear.x, self.limited_cmd.linear.x,
self.accel_lim, dt)
self.limited_cmd.linear.y = slew_limit(
self.motion_cmd.linear.y, self.limited_cmd.linear.y,
self.accel_lim, dt)
self.limited_cmd.angular.z = slew_limit(
self.motion_cmd.angular.z, self.limited_cmd.angular.z,
self.yaw_accel_lim, dt)
if ((rospy.get_time() - self.last_motion_command_time) <
2.0):
self.motion_pub.publish(self.limited_cmd)
def _movo_teleop(self, joyMessage):
self._parse_joy_input(joyMessage)
dt = rospy.get_time() - self.last_joy
self.last_joy = rospy.get_time()
if self.button_state['base_ctl']:
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['arm_ctl_right']:
self.run_arm_ctl_right = True
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['arm_ctl_left']:
self.run_arm_ctl_right = False
self.run_arm_ctl_left = True
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
elif self.button_state['pan_tilt_ctl']:
self.run_arm_ctl = False
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = True
self._init_pan_tilt = True
if self.button_state['estop']:
self.run_arm_ctl = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = False
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
self.arm_pub[0].publish(arm_cmd)
self.arm_pub[1].publish(arm_cmd)
if self.button_state['home_arms']:
if not (self.homing_sent):
tmp = Bool()
tmp.data = True
self.home_arm_pub.publish(tmp)
self.homing_sent = True
self.homing_start_time = rospy.get_time()
if (self.homing_sent):
if ((rospy.get_time() - self.homing_start_time) >
10.0) and not self.button_state['home_arms']:
self.homing_sent = False
if self.button_state['estop']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = DISABLE_REQUEST
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
if self.run_arm_ctl_right or self.run_arm_ctl_left:
arm_cmd = JacoCartesianVelocityCmd()
arm_cmd.header.stamp = rospy.get_rostime()
arm_cmd.header.frame_id = ''
gripper_cmd = GripperCmd()
kg_cmd = Float32()
if self.run_arm_ctl_right:
arm_idx = 0
else:
arm_idx = 1
if self.button_state['dead_man']:
if 0 == arm_idx:
arm_cmd.x = self.axis_value[
'left_right'] * -self.arm_vel_lim
else:
arm_cmd.x = self.axis_value['left_right'] * self.arm_vel_lim
arm_cmd.z = self.axis_value['for_aft'] * self.arm_vel_lim
if not self.button_state['man_ovvrd']:
arm_cmd.y = self.axis_value['twist'] * self.arm_vel_lim
else:
dt = rospy.get_time() - self.last_arm_update
self.lincmd.desired_position_m += (
self.axis_value['twist'] * self.lin_act_vel_lim) * dt
if (self.lincmd.desired_position_m > 0.464312):
self.lincmd.desired_position_m = 0.464312
elif self.lincmd.desired_position_m < 0.0:
self.lincmd.desired_position_m = 0.0
self.lincmd.header.stamp = rospy.get_rostime()
self.lincmd.header.frame_id = ''
self.linpub.publish(self.lincmd)
self.lincmd.header.seq += 1
self.last_arm_update = rospy.get_time()
arm_cmd.theta_y = self.axis_value['dpad_ud'] * 100.0
arm_cmd.theta_x = self.axis_value['dpad_lr'] * 100.0
if self.button_state['wrist1']:
arm_cmd.theta_z = 100.0
elif self.button_state['wrist2']:
arm_cmd.theta_z = -100.0
if self.button_state['standby']:
kg_cmd.data = 5000.0
elif self.button_state['tractor']:
kg_cmd.data = -5000.0
else:
kg_cmd.data = 0.0
gripper_val = (self.axis_value['flipper'] + 1.0) / 2.0
if abs(self._last_gripper_val - gripper_val) > 0.05:
gripper_cmd.position = gripper_val * 0.085
gripper_cmd.speed = 0.05
gripper_cmd.force = 100.0
self.gripper_pub[arm_idx].publish(gripper_cmd)
self._last_gripper_val = gripper_val
self.arm_pub[arm_idx].publish(arm_cmd)
self.kgripper_pub[arm_idx].publish(kg_cmd)
elif self.run_pan_tilt_ctl:
vel_cmd = [0.0, 0.0]
if self.button_state['dead_man']:
vel_cmd[0] = -self.axis_value['twist'] * self.pt_vel_lim
vel_cmd[1] = -self.axis_value['for_aft'] * self.pt_vel_lim
self.pt_cmds.pan_cmd.pos_rad += vel_cmd[0] * dt
self.pt_cmds.tilt_cmd.pos_rad += vel_cmd[1] * dt
self.pt_cmds.pan_cmd.pos_rad = limit_f(
self.pt_cmds.pan_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.tilt_cmd.pos_rad = limit_f(
self.pt_cmds.tilt_cmd.pos_rad, (math.pi / 2.0))
self.pt_cmds.pan_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.pt_cmds.tilt_cmd.vel_rps = 50.0 * (math.pi / 180.0)
self.p_pub.publish(self.pt_cmds)
else:
if self.button_state['estop']:
#handled at top of function
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
elif self.button_state['standby']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = STANDBY_REQUEST
elif self.button_state['tractor']:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = TRACTOR_REQUEST
else:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
if ('GENERAL_PURPOSE_CMD_NONE' != self.cfg_cmd.gp_cmd):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = True
elif (True == self.send_cmd_none):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = False
elif (False == self.send_cmd_none):
if self.button_state['dead_man']:
self.motion_cmd.linear.x = (self.axis_value['for_aft'] *
self.x_vel_limit_mps)
self.motion_cmd.linear.y = (self.axis_value['left_right'] *
self.y_vel_limit_mps)
self.motion_cmd.angular.z = (self.axis_value['twist'] *
self.yaw_rate_limit_rps)
self.last_motion_command_time = rospy.get_time()
else:
self.motion_cmd.linear.x = 0.0
self.motion_cmd.linear.y = 0.0
self.motion_cmd.angular.z = 0.0
if (dt >= 0.01):
self.limited_cmd.linear.x = slew_limit(
self.motion_cmd.linear.x, self.limited_cmd.linear.x,
self.accel_lim, dt)
self.limited_cmd.linear.y = slew_limit(
self.motion_cmd.linear.y, self.limited_cmd.linear.y,
self.accel_lim, dt)
self.limited_cmd.angular.z = slew_limit(
self.motion_cmd.angular.z, self.limited_cmd.angular.z,
self.yaw_accel_lim, dt)
if ((rospy.get_time() - self.last_motion_command_time) <
2.0):
self.motion_pub.publish(self.limited_cmd)
if self.button_state[
'man_ovvrd'] and self.button_state['man_ovvrd']:
self.override_pub.publish(self.motion_cmd)