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 __init__(self):
rospy.on_shutdown(self._shutdown)
self.last_tracking_time = rospy.get_time()
# camera related parameters
self.max_pan_view_angle = np.radians(30)
self.max_tilt_view_angle = np.radians(20)
self.max_dt_lag = 0.5
self.list_faces = []
self.nearest_face = Face()
self.head_cmd_srv = rospy.ServiceProxy("/head_cmd_mux/select",
MuxSelect)
rospy.wait_for_service('/head_cmd_mux/select')
head_cmd_request = MuxSelectRequest()
head_cmd_request.topic = '/movo/head/face_tracking/cmd'
self.head_cmd_srv.call(head_cmd_request)
self.sync_head_pose_mutex = threading.Lock()
self.face_detector_sub = rospy.Subscriber("/face_detector/faces_cloud",
PointCloud,
self._face_tracking)
# self.face_detector_sub = rospy.Subscriber("/face_detector/people_tracker_measuremes_array", PositionMeasurementArray, self._face_tracking)
self.head_joint_state_sub = rospy.Subscriber("/movo/head/joint_states",
JointState,
self._joint_state_cb)
self.head_motion_pub = rospy.Publisher("/movo/head/face_tracking/cmd",
PanTiltCmd,
queue_size=10)
self.head_cmd = PanTiltCmd()
self.face_found_pub = rospy.Publisher("/face_detector/nearest_face",
FaceFound,
queue_size=10)
self.face_found_msg = FaceFound()
self.face_found_msg.header.seq = 0
self.face_found_msg.header.stamp = rospy.get_rostime()
self.face_found_msg.header.frame_id = "camera_color_optical_frame"
# self.pantilt_vel_lim = rospy.get_param('~sim_teleop_pan_tilt_vel_limit', 0.524)
self.pan_vel_lim = self.max_pan_view_angle # np.radians(30) = 0.524
self.tilt_vel_lim = self.max_tilt_view_angle
# pose increment inside deadzone, no need to send cmd
self.pantilt_pose_deadzone = np.radians(2.0)
# head searching face motion
self.head_searching_wait_time = 5.0
self.pan_searching_increment = np.radians(3.0)
self.pan_searching_limit = np.radians(60.0)
self.tilt_searching_increment = np.radians(10.0)
self.tilt_searching_limit = np.radians(30.0)
self.head_searching_thread = threading.Thread(
target=self.head_searching_cb)
self.head_searching_thread.start()
# rate.sleep()
rospy.loginfo("FaceTracking initialization")
rospy.spin()
def _shutdown(self):
head_cmd_request = MuxSelectRequest()
head_cmd_request.topic = '/movo/head/teleop/cmd'
self.head_cmd_srv.call(head_cmd_request)
def change_trajectory(cls, source):
return cls._change_trajectory(MuxSelectRequest(source))
def change_wrench(cls, source):
return cls._change_wrench(MuxSelectRequest(source))
def change_wrench(self, source):
return self._change_wrench(MuxSelectRequest(source))
