def _command_joints(self, joint_names, point):
if self._server.is_preempt_requested() or not self.robot_is_enabled():
rospy.loginfo("%s: Trajectory Preempted" % (self._action_name, ))
self._server.set_preempted()
self._command_stop()
return False
self.pos_targets = point.positions
deltas = self._get_current_errors(joint_names)
for delta in deltas:
if ((math.fabs(delta[1]) >= self._path_thresh[delta[0]]
and self._path_thresh[delta[0]] >= 0.0)
) or not self.robot_is_enabled():
rospy.logerr("%s: Exceeded Error Threshold on %s: %s" % (
self._action_name,
delta[0],
str(delta[1]),
))
self._result.error_code = self._result.PATH_TOLERANCE_VIOLATED
self._server.set_aborted(self._result)
self._command_stop()
return False
cmdz = PanTiltCmd()
cmdz.pan_cmd.pos_rad = point.positions[0]
cmdz.tilt_cmd.pos_rad = point.positions[1]
cmdz.pan_cmd.vel_rps = 10000.0
cmdz.tilt_cmd.vel_rps = 10000.0
if self._alive:
self._cmd_pub.publish(cmdz)
actual = self._get_current_position(joint_names)
return True
def __init__(self):
self.is_sim = rospy.get_param('~sim', True)
self.lincmd = LinearActuatorCmd()
if (False == self.is_sim):
"""
Subscribe to the configuration message
"""
self.config_updated = False
rospy.Subscriber("/movo/feedback/active_configuration",
Configuration, self._update_configuration_limits)
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) <
10.0) and (False == self.config_updated):
rospy.sleep(0.05)
if (False == self.config_updated):
rospy.logerr(
"Timed out waiting for Movo feedback topics make sure the driver is running"
)
sys.exit(0)
return
"""
Initialize the linear actuator position if this is the real system
"""
movo_dynamics = rospy.wait_for_message("/movo/feedback/dynamics",
Dynamics)
self.lincmd.desired_position_m = movo_dynamics.linear_actuator_position_m
else:
self.x_vel_limit_mps = rospy.get_param(
'~sim_teleop_x_vel_limit_mps', 0.5)
self.y_vel_limit_mps = rospy.get_param(
'~sim_teleop_y_vel_limit_mps', 0.5)
self.yaw_rate_limit_rps = rospy.get_param(
'~sim_teleop_yaw_rate_limit_rps', 0.5)
self.accel_lim = rospy.get_param('~sim_teleop_accel_lim', 0.5)
self.yaw_accel_lim = rospy.get_param('~sim_teleop_yaw_accel_lim',
1.0)
self.arm_vel_lim = rospy.get_param('~sim_teleop_arm_vel_limit',
0.1)
self.pt_vel_lim = rospy.get_param('~sim_teleop_pan_tilt_vel_limit',
0.524)
self.lin_act_vel_lim = rospy.get_param(
'~sim_teleop_linear_actuator_vel_limit', 0.05)
"""
Set the mapping for the various commands. Take the wired microsoft xbox 360
"""
self.ctrl_map = dict({
'momentary': {
'MAP_BASE_A': {
'is_button': True,
'index': 1,
'set_val': 1
},
'MAP_rARM_B': {
'is_button': True,
'index': 2,
'set_val': 1
},
'MAP_lARM_X': {
'is_button': True,
'index': 3,
'set_val': 1
},
'MAP_HEAD_Y': {
'is_button': True,
'index': 4,
'set_val': 1
},
'MAP_lWRIST_LB': {
'is_button': True,
'index': 5,
'set_val': 1
},
'MAP_rWRIST_RB': {
'is_button': True,
'index': 6,
'set_val': 1
},
'MAP_TORS_back_': {
'is_button': True,
'index': 7,
'set_val': 1
},
'MAP_HOME_start': {
'is_button': True,
'index': 8,
'set_val': 1
},
'MAP_eSTOP_power': {
'is_button': True,
'index': 9,
'set_val': 1
},
'MAP_TRACT_lstick': {
'is_button': True,
'index': 10,
'set_val': 1
},
'MAP_stby_rstick': {
'is_button': True,
'index': 11,
'set_val': 1
}
},
'axis': {
'MAP_TWIST_LR_stickleft': {
'index': 1,
'invert_axis': False
},
'MAP_TWIST_UD_stickleft': {
'index': 2,
'invert_axis': False
},
'MAP_TRIGGER_LT': {
'index': 3,
'invert_axis': False
},
'MAP_TWIST_LR_stickright': {
'index': 4,
'invert_axis': False
},
'MAP_TWIST_UD_stickright': {
'index': 5,
'invert_axis': False
},
'MAP_TRIGGER_RT': {
'index': 6,
'invert_axis': False
},
'MAP_CROSS_LR': {
'index': 7,
'invert_axis': False
},
'MAP_CROSS_UD': {
'index': 8,
'invert_axis': False
}
}
})
"""
Initialize the debounce logic states
"""
self.db_cnt = dict()
self.axis_value = dict()
self.button_state = dict()
for key, value in self.ctrl_map.iteritems():
if key == 'momentary':
for key, value2 in value.iteritems():
self.db_cnt[key] = 0
self.button_state[key] = False
else:
self.db_cnt[key] = 0
self.axis_value[key] = 0.0
self.send_cmd_none = False
self.no_motion_commands = True
self.last_motion_command_time = 0.0
self.last_joy = rospy.get_time()
self._last_gripper_val = 0.0
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = True
self._last_angles = [0.0, 0.0]
self._pt_pos_cmd = [0.0, 0.0]
self.cfg_cmd = ConfigCmd()
self.cfg_pub = rospy.Publisher('/movo/gp_command',
ConfigCmd,
queue_size=10)
self.motion_cmd = Twist()
self.limited_cmd = Twist()
self.motion_pub = rospy.Publisher('/movo/teleop/cmd_vel',
Twist,
queue_size=10)
self.override_pub = rospy.Publisher("/movo/manual_override/cmd_vel",
Twist,
queue_size=10)
self.linpub = rospy.Publisher("/movo/linear_actuator_cmd",
LinearActuatorCmd,
queue_size=1)
self.arm_pub = [0] * 2
self.gripper_pub = [0] * 2
self.kgripper_pub = [0] * 2
self.home_arm_pub = rospy.Publisher('/movo/home_arms',
Bool,
queue_size=10)
self.last_home_req = False
self.home_req_timeout = 0.0
self.homing_sent = False
self.arm_pub[0] = rospy.Publisher('/movo/right_arm/cartesian_vel_cmd',
JacoCartesianVelocityCmd,
queue_size=10)
self.gripper_pub[0] = rospy.Publisher('/movo/right_gripper/cmd',
GripperCmd,
queue_size=10)
self.kgripper_pub[0] = rospy.Publisher('/movo/right_gripper/vel_cmd',
Float32,
queue_size=10)
self.arm_pub[1] = rospy.Publisher('/movo/left_arm/cartesian_vel_cmd',
JacoCartesianVelocityCmd,
queue_size=10)
self.gripper_pub[1] = rospy.Publisher('/movo/left_gripper/cmd',
GripperCmd,
queue_size=10)
self.kgripper_pub[1] = rospy.Publisher('/movo/left_gripper/vel_cmd',
Float32,
queue_size=10)
self.p_pub = rospy.Publisher("/movo/head/teleop/cmd",
PanTiltCmd,
queue_size=100)
self.pt_cmds = PanTiltCmd()
self.head_cmd_srv = rospy.ServiceProxy("/head_cmd_mux/select",
MuxSelect)
self.head_cmd_source = ['teleop', 'face_tracking']
self.head_cmd_current_source_index = -1
self.teleop_control_mode_pub = rospy.Publisher(
"/movo/teleop/control_mode", String, queue_size=10)
self.teleop_control_mode_msg = String()
rospy.Subscriber('/joy', Joy, self._movo_teleop)
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 __init__(self):
self.is_sim = rospy.get_param('~sim', True)
self.lincmd = LinearActuatorCmd()
if (False == self.is_sim):
"""
Subscribe to the configuration message
"""
self.config_updated = False
rospy.Subscriber("/movo/feedback/active_configuration",
Configuration, self._update_configuration_limits)
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) <
10.0) and (False == self.config_updated):
rospy.sleep(0.05)
if (False == self.config_updated):
rospy.logerr(
"Timed out waiting for Movo feedback topics make sure the driver is running"
)
sys.exit(0)
return
"""
Initialize the linear actuator position if this is the real system
"""
movo_dynamics = rospy.wait_for_message("/movo/feedback/dynamics",
Dynamics)
self.lincmd.desired_position_m = movo_dynamics.linear_actuator_position_m
else:
self.x_vel_limit_mps = rospy.get_param(
'~sim_teleop_x_vel_limit_mps', 0.5)
self.y_vel_limit_mps = rospy.get_param(
'~sim_teleop_y_vel_limit_mps', 0.5)
self.yaw_rate_limit_rps = rospy.get_param(
'~sim_teleop_yaw_rate_limit_rps', 0.5)
self.accel_lim = rospy.get_param('~sim_teleop_accel_lim', 0.5)
self.yaw_accel_lim = rospy.get_param('~sim_teleop_yaw_accel_lim',
1.0)
self.arm_vel_lim = rospy.get_param('~sim_teleop_arm_vel_limit',
0.1)
self.pt_vel_lim = rospy.get_param('~sim_teleop_pan_tilt_vel_limit',
0.524)
self.lin_act_vel_lim = rospy.get_param(
'~sim_teleop_linear_actuator_vel_limit', 0.05)
"""
Set the mapping for the various commands
"""
self.ctrl_map = dict({
'momentary': {
'dead_man': {
'is_button': True,
'index': 1,
'set_val': 1
},
'man_ovvrd': {
'is_button': True,
'index': 2,
'set_val': 1
},
'standby': {
'is_button': True,
'index': 3,
'set_val': 1
},
'tractor': {
'is_button': True,
'index': 4,
'set_val': 1
},
'wrist1': {
'is_button': True,
'index': 5,
'set_val': 1
},
'wrist2': {
'is_button': True,
'index': 6,
'set_val': 1
},
'estop': {
'is_button': True,
'index': 7,
'set_val': 1
},
'home_arms': {
'is_button': True,
'index': 8,
'set_val': 1
},
'pan_tilt_ctl': {
'is_button': True,
'index': 9,
'set_val': 1
},
'base_ctl': {
'is_button': True,
'index': 10,
'set_val': 1
},
'arm_ctl_right': {
'is_button': True,
'index': 11,
'set_val': 1
},
'arm_ctl_left': {
'is_button': True,
'index': 12,
'set_val': 1
}
},
'axis': {
'left_right': {
'index': 1,
'invert_axis': False
},
'for_aft': {
'index': 2,
'invert_axis': False
},
'twist': {
'index': 3,
'invert_axis': False
},
'flipper': {
'index': 4,
'invert_axis': False
},
'dpad_lr': {
'index': 5,
'invert_axis': False
},
'dpad_ud': {
'index': 6,
'invert_axis': False
}
}
})
"""
Initialize the debounce logic states
"""
self.db_cnt = dict()
self.axis_value = dict()
self.button_state = dict()
for key, value in self.ctrl_map.iteritems():
if key == 'momentary':
for key, value2 in value.iteritems():
self.db_cnt[key] = 0
self.button_state[key] = False
else:
self.db_cnt[key] = 0
self.axis_value[key] = 0.0
self.send_cmd_none = False
self.no_motion_commands = True
self.last_motion_command_time = 0.0
self.last_joy = rospy.get_time()
self._last_gripper_val = 0.0
self.run_arm_ctl_right = False
self.run_arm_ctl_left = False
self.run_pan_tilt_ctl = False
self._init_pan_tilt = True
self._last_angles = [0.0, 0.0]
self._pt_pos_cmd = [0.0, 0.0]
self.cfg_cmd = ConfigCmd()
self.cfg_pub = rospy.Publisher('/movo/gp_command',
ConfigCmd,
queue_size=10)
self.motion_cmd = Twist()
self.limited_cmd = Twist()
self.motion_pub = rospy.Publisher('/movo/teleop/cmd_vel',
Twist,
queue_size=10)
self.override_pub = rospy.Publisher("/movo/manual_override/cmd_vel",
Twist,
queue_size=10)
self.linpub = rospy.Publisher("/movo/linear_actuator_cmd",
LinearActuatorCmd,
queue_size=1)
self.arm_pub = [0] * 2
self.gripper_pub = [0] * 2
self.kgripper_pub = [0] * 2
self.home_arm_pub = rospy.Publisher('/movo/home_arms',
Bool,
queue_size=10)
self.last_home_req = False
self.home_req_timeout = 0.0
self.homing_sent = False
self.arm_pub[0] = rospy.Publisher('/movo/right_arm/cartesian_vel_cmd',
JacoCartesianVelocityCmd,
queue_size=10)
self.gripper_pub[0] = rospy.Publisher('/movo/right_gripper/cmd',
GripperCmd,
queue_size=10)
self.kgripper_pub[0] = rospy.Publisher('/movo/right_gripper/vel_cmd',
Float32,
queue_size=10)
self.arm_pub[1] = rospy.Publisher('/movo/left_arm/cartesian_vel_cmd',
JacoCartesianVelocityCmd,
queue_size=10)
self.gripper_pub[1] = rospy.Publisher('/movo/left_gripper/cmd',
GripperCmd,
queue_size=10)
self.kgripper_pub[1] = rospy.Publisher('/movo/left_gripper/vel_cmd',
Float32,
queue_size=10)
self.p_pub = rospy.Publisher("/movo/head/cmd",
PanTiltCmd,
queue_size=100)
self.pt_cmds = PanTiltCmd()
rospy.Subscriber('/joy', Joy, self._movo_teleop)
def __init__(self, movo_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run MOVO Linear actuator command interface
"""
self._cmd_buffer = multiprocessing.Queue()
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6237),
self.tx_queue_,
self.rx_queue_,
max_packet_size=KINOVA_ACTUATOR_RSP_SIZE_BYTES)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for MOVO pan_tilt...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = PanTiltCmd()
self._last_cmd = JointTrajectoryPoint()
self._init_cmds = True
self.s = rospy.Subscriber("/movo/head/cmd", PanTiltCmd,
self._add_motion_command_to_queue)
self._jcc = rospy.Subscriber("/movo/head_controller/command",
JointTrajectory,
self._add_traj_command_to_queue)
self.actuator_data = PanTiltFdbk()
self.actuator_pub = rospy.Publisher("/movo/head/data",
PanTiltFdbk,
queue_size=10)
self.js = JointState()
self.js_pub = rospy.Publisher("/movo/head/joint_states",
JointState,
queue_size=10)
self._jcs = JointTrajectoryControllerState()
self._jcs_pub = rospy.Publisher("/movo/head_controller/state",
JointTrajectoryControllerState,
queue_size=10)
self._jc_srv = rospy.Service('/movo/head_controller/query_state',
QueryTrajectoryState,
self._handle_state_query)
"""
Start the receive handler thread
"""
self.need_to_terminate = False
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.get_time()
self._rcv_thread = threading.Thread(target=self._run)
self._rcv_thread.start()
self._t1 = rospy.Timer(rospy.Duration(0.01),
self._update_command_queue)
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop MOVO pan_tilt communication stream")
self.Shutdown()
return
"""
Start streaming continuous data
"""
rospy.loginfo("Starting the data stream")
if (False == self._continuous_data(True)):
rospy.logerr("Could not start MOVO pan_tilt communication stream")
self.Shutdown()
return
rospy.loginfo("Movo Pan-Tilt Head Driver is up and running")
self.init_success = True