def __init__(self, movo_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run MOVO Linear actuator command interface
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6236),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr(
"Could not open socket for MOVO linear actuator...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = LinearActuatorCmd()
self.s = rospy.Subscriber("/movo/linear_actuator_cmd",
LinearActuatorCmd,
self._add_motion_command_to_queue)
self.init_success = True
def __init__(self, num_grippers=1, movo_ip='10.66.171.5', rcv_timeout=0.1):
self._shutdown_driver = False
self._rcv_timeout = rcv_timeout
"""
Create the thread to run MOVO Linear actuator command interface
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6238),
self.tx_queue_,
self.rx_queue_,
max_packet_size=R85_PACKET_SIZE_BYTES)
self._gripper = []
self._num_grippers = num_grippers
for i in range(self._num_grippers):
self._gripper.append(GripperIO(i))
self.init_success = True
class LinearActuator(object):
def __init__(self, movo_ip='10.66.171.5'):
self.init_success = False
"""
Create the thread to run MOVO Linear actuator command interface
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6236),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr(
"Could not open socket for MOVO linear actuator...exiting")
self.Shutdown()
return
"""
Initialize the publishers and subscribers for the node
"""
self.cmd_data = LinearActuatorCmd()
self.s = rospy.Subscriber("/movo/linear_actuator_cmd",
LinearActuatorCmd,
self._add_motion_command_to_queue)
self.init_success = True
def Shutdown(self):
rospy.loginfo("Shutting down the linear actuator command driver...")
self.s.unregister()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def UpdateVelLimit(self, new_limit):
cmds = [
LINEAR_ACTUATOR_VELOCITY_LIMIT_CMD_ID,
[convert_float_to_u32(new_limit)]
]
self._add_command_to_queue(cmds)
def _add_command_to_queue(self, command):
if (rospy.is_shutdown()):
return
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _add_motion_command_to_queue(self, command):
if (rospy.is_shutdown()):
return
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [
LINEAR_ACTUATOR_POSITION_CMD_ID,
[convert_float_to_u32(command.desired_position_m)]
]
self._add_command_to_queue(cmds)
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
class PanTiltIO(object):
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
def Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo(
"Movo pan_tilt has called the Shutdown method, terminating")
self.js_pub.unregister()
self.actuator_pub.unregister()
self.s.unregister()
self._jcs_pub.unregister()
self._jcc.unregister()
self._jc_srv.shutdown()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader], [], [], 0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _handle_state_query(self, req):
tmp = QueryTrajectoryStateResponse()
tmp.name = ['pan_joint', 'tilt_joint']
tmp.position = [
self.actuator_data.pan.pos_rad, self.actuator_data.tilt.pos_rad
]
tmp.velocity = [
self.actuator_data.pan.vel_rps, self.actuator_data.tilt.vel_rps
]
tmp.acceleration = [0.0, 0.0]
return tmp
def _update_command_queue(self, event):
"""
Send it
"""
if not self._cmd_buffer.empty():
tmp = self._cmd_buffer.get_nowait()
cmds = [
KINOVA_ACTUATOR_CMD_ID,
[
convert_float_to_u32(tmp.positions[0]),
convert_float_to_u32(tmp.velocities[0]),
convert_float_to_u32(tmp.accelerations[0]),
convert_float_to_u32(tmp.positions[1]),
convert_float_to_u32(tmp.velocities[1]),
convert_float_to_u32(tmp.accelerations[1])
]
]
self._last_cmd = tmp
cmd_bytes = generate_cmd_bytes(cmds)
self.tx_queue_.put(cmd_bytes)
def _add_motion_command_to_queue(self, command):
tmp = JointTrajectory()
tmp.header = command.header
tmp.joint_names = ['pan_joint', 'pan_joint']
tmp.points = [JointTrajectoryPoint()]
tmp.points[0].positions = [
command.pan_cmd.pos_rad, command.tilt_cmd.pos_rad
]
tmp.points[0].velocities = [
command.pan_cmd.vel_rps, command.tilt_cmd.vel_rps
]
tmp.points[0].accelerations = [
command.pan_cmd.acc_rps2, command.tilt_cmd.acc_rps2
]
self._cmd_buffer.put(tmp.points[0])
def _add_traj_command_to_queue(self, msg):
for pnt in msg.points:
self._cmd_buffer.put(pnt)
def _add_config_command_to_queue(self, gp_cmd, gp_param):
try:
cmds = [EIB_GENERAL_CMD_ID, [gp_cmd, gp_param]]
cmd_bytes = generate_cmd_bytes(cmds)
self.tx_queue_.put(cmd_bytes)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _handle_rsp(self, data_bytes):
valid_data = validate_response(data_bytes,
KINOVA_ACTUATOR_RSP_SIZE_BYTES)
if (False == valid_data):
self.last_rsp_rcvd = rospy.get_time()
rospy.logerr("bad movo pan_tilt data packet")
return
self.last_rsp_rcvd = rospy.get_time()
data_bytes = data_bytes[2:]
rsp_data = array.array('I', data_bytes.tostring()).tolist()
rsp_data = rsp_data[:(len(rsp_data) - 1)]
rsp = [convert_u32_to_float(i) for i in rsp_data]
self.actuator_data.header.stamp = rospy.get_rostime()
self.actuator_data.header.seq += 1
for i in range(2):
if (i == 0):
self.actuator_data.pan.current = (rsp[10 * i])
self.actuator_data.pan.pos_rad = (rsp[10 * i + 1])
self.actuator_data.pan.vel_rps = (rsp[10 * i + 2])
self.actuator_data.pan.torque_nm = (rsp[10 * i + 3])
self.actuator_data.pan.pwm = (rsp[10 * i + 4])
self.actuator_data.pan.encoder_rad = (rsp[10 * i + 5])
self.actuator_data.pan.accel.x = (rsp[10 * i + 6])
self.actuator_data.pan.accel.y = (rsp[10 * i + 7])
self.actuator_data.pan.accel.z = (rsp[10 * i + 8])
self.actuator_data.pan.temperature_degC = (rsp[10 * i + 9])
else:
self.actuator_data.tilt.current = (rsp[10 * i])
self.actuator_data.tilt.pos_rad = (rsp[10 * i + 1])
self.actuator_data.tilt.vel_rps = (rsp[10 * i + 2])
self.actuator_data.tilt.torque_nm = (rsp[10 * i + 3])
self.actuator_data.tilt.pwm = (rsp[10 * i + 4])
self.actuator_data.tilt.encoder_rad = (rsp[10 * i + 5])
self.actuator_data.tilt.accel.x = (rsp[10 * i + 6])
self.actuator_data.tilt.accel.y = (rsp[10 * i + 7])
self.actuator_data.tilt.accel.z = (rsp[10 * i + 8])
self.actuator_data.tilt.temperature_degC = (rsp[10 * i + 9])
if not rospy.is_shutdown():
self.actuator_pub.publish(self.actuator_data)
self.js.header.stamp = self.actuator_data.header.stamp
self.js.name = ['pan_joint', 'tilt_joint']
self.js.position = [
self.actuator_data.pan.pos_rad, self.actuator_data.tilt.pos_rad
]
self.js.velocity = [
self.actuator_data.pan.vel_rps, self.actuator_data.tilt.vel_rps
]
self.js.effort = [
self.actuator_data.pan.torque_nm,
self.actuator_data.tilt.torque_nm
]
self.js_pub.publish(self.js)
self._jcs.header.stamp = self.actuator_data.header.stamp
self._jcs.joint_names = ['pan_joint', 'tilt_joint']
self._jcs.desired = self._last_cmd
self._jcs.actual.positions = self.js.position
self._jcs.actual.velocities = self.js.velocity
self._jcs.actual.accelerations = [0.0, 0.0]
try:
self._jcs.error.positions = list(
map(operator.sub, self._jcs.desired.positions,
self._jcs.actual.positions))
self._jcs.error.velocities = list(
map(operator.sub, self._jcs.desired.velocities,
self._jcs.actual.velocities))
except:
self._jcs.error.positions = [0.0] * 2
self._jcs.error.velocities = [0.0] * 2
self._jcs.desired = self._jcs.actual
self._last_cmd = self._jcs.actual
self._jcs.error.accelerations = [0.0, 0.0]
self._jcs_pub.publish(self._jcs)
rospy.logdebug("feedback received from movo")
def _continuous_data(self, start_cont):
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) < 3.0) and (False == ret):
self._add_config_command_to_queue(0, 1)
r.sleep()
if ((rospy.get_time() - self.last_rsp_rcvd) < 0.05):
ret = True
else:
start_time = rospy.get_time()
while ((rospy.get_time() - start_time) < 3.0) and (False == ret):
self._add_config_command_to_queue(0, 0)
rospy.sleep(0.6)
if ((rospy.get_time() - self.last_rsp_rcvd) > 0.5):
ret = True
return ret
class Robotiq85Gripper:
def __init__(self, num_grippers=1, movo_ip='10.66.171.5', rcv_timeout=0.1):
self._shutdown_driver = False
self._rcv_timeout = rcv_timeout
"""
Create the thread to run MOVO Linear actuator command interface
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 6238),
self.tx_queue_,
self.rx_queue_,
max_packet_size=R85_PACKET_SIZE_BYTES)
self._gripper = []
self._num_grippers = num_grippers
for i in range(self._num_grippers):
self._gripper.append(GripperIO(i))
self.init_success = True
def shutdown(self):
self._shutdown_driver = True
rospy.loginfo(
"Movo R85 gripper has called the Shutdown method, terminating")
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _transact(self, cmd, rx_bytes=0):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = [0] * 68
cmd_bytes[0] = 0
cmd_bytes[1] = 0
cmd_bytes[2] = (len(cmd) & 0xFF)
cmd_bytes[3] = (rx_bytes & 0xFF)
j = 4
for b in cmd:
cmd_bytes[j] = b
j += 1
cmd_bytes = array.array('B', cmd_bytes)
"""
Generate the CRC for the command bytes
"""
crc = calc_crc32(cmd_bytes)
add_bytes(cmd_bytes, crc, 32)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
rsp_data = None
try:
result = select.select([self.rx_queue_._reader], [], [],
self._rcv_timeout)
if len(result[0]) > 0:
rsp = result[0][0].recv()
if (valid_crc32(rsp)) and (len(rsp) == 72):
rsp_data = rsp[4:rx_bytes + 4]
except:
pass
return rsp_data
def process_act_cmd(self, dev=0):
if (dev >= self._num_grippers) or (self._shutdown_driver):
return False
rsp = self._transact(self._gripper[dev].act_cmd, 8)
if (None == rsp):
return False
return verify_modbus_rtu_crc(rsp)
def process_stat_cmd(self, dev=0):
if (dev >= self._num_grippers) or (self._shutdown_driver):
return False
rsp = self._transact(self._gripper[dev].stat_cmd, 21)
if (None == rsp):
return False
return self._gripper[dev].parse_rsp(rsp)
def activate_gripper(self, dev=0):
if (dev >= self._num_grippers):
return
self._gripper[dev].activate_gripper()
def deactivate_gripper(self, dev=0):
if (dev >= self._num_grippers):
return
self._gripper[dev].deactivate_gripper()
def activate_emergency_release(self, dev=0, open_gripper=True):
if (dev >= self._num_grippers):
return
self._gripper[dev].activate_emergency_release(open_gripper)
def deactivate_emergency_release(self, dev=0):
if (dev >= self._num_grippers):
return
self._gripper[dev].deactivate_emergency_release()
def goto(self, dev=0, pos=0.0, vel=1.0, force=1.0):
if (dev >= self._num_grippers):
return
self._gripper[dev].goto(pos, vel, force)
def stop(self, dev=0):
if (dev >= self._num_grippers):
return
self._gripper[dev].stop()
def is_ready(self, dev=0):
if (dev >= self._num_grippers):
return False
return self._gripper[dev].is_ready()
def is_reset(self, dev=0):
if (dev >= self._num_grippers):
return False
return self._gripper[dev].is_reset()
def is_moving(self, dev=0):
if (dev >= self._num_grippers):
return False
return self._gripper[dev].is_moving()
def is_stopped(self, dev=0):
if (dev >= self._num_grippers):
return False
return self._gripper[dev].is_moving()
def object_detected(self, dev=0):
if (dev >= self._num_grippers):
return False
return self._gripper[dev].object_detected()
def get_fault_status(self, dev=0):
if (dev >= self._num_grippers):
return 0
return self._gripper[dev].get_fault_status()
def get_pos(self, dev=0):
if (dev >= self._num_grippers):
return 0
return self._gripper[dev].get_pos()
def get_req_pos(self, dev=0):
if (dev >= self._num_grippers):
return 0
return self._gripper[dev].get_req_pos()
def get_current(self, dev=0):
if (dev >= self._num_grippers):
return 0
return self._gripper[dev].get_current()
def __init__(self,movo_ip='10.66.171.5'):
"""
Variables to track communication frequency for debugging
"""
self.summer=0
self.samp = 0
self.avg_freq = 0.0
self.start_frequency_samp = False
self.need_to_terminate = False
self.flush_rcvd_data=True
self.update_base_local_planner = False
self.last_move_base_update = rospy.get_time()
"""
Initialize the publishers for MOVO
"""
self.movo_data = MOVO_DATA()
"""
Start the thread for the linear actuator commands
"""
self._linear = LinearActuator(movo_ip)
if (False == self._linear.init_success):
rospy.logerr("Could not initialize the linear actuator interface! exiting...")
return
"""
Initialize faultlog related items
"""
self.is_init = True
self.extracting_faultlog = False
"""
Initialize the dynamic reconfigure server for MOVO
"""
self.param_server_initialized = False
self.dyn_reconfigure_srv = Server(movoConfig, self._dyn_reconfig_callback)
"""
Wait for the parameter server to set the configs and then set the IP address from that.
Note that this must be the current ethernet settings of the platform. If you want to change it
set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the
the ethernet settings at launch to the new ones and relaunch
"""
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == self.param_server_initialized):
r.sleep()
if (False == self.param_server_initialized):
rospy.logerr("Parameter server not found, you must pass an initial yaml in the launch! exiting...")
return
"""
Create the thread to run MOVO communication
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 8080),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for MOVO...")
self.comm.close()
return
"""
Initialize the publishers and subscribers for the node
"""
self.s = [0]*4
self.faultlog_pub = rospy.Publisher('/movo/feedback/faultlog', Faultlog, queue_size=10,latch=True)
self.s[0] = rospy.Subscriber("/movo/cmd_vel", Twist, self._add_motion_command_to_queue)
self.s[1] = rospy.Subscriber("/movo/gp_command",ConfigCmd,self._add_config_command_to_queue)
self.s[2] = rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates",Config,self._update_move_base_params)
self.s[3] = rospy.Subscriber("/movo/motion_test_cmd",MotionTestCmd,self._add_motion_test_command_to_queue)
"""
Start the receive handler thread
"""
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.Time.now().to_sec()
self._rcv_thread = threading.Thread(target = self._run)
self._rcv_thread.start()
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop MOVO communication stream")
self.Shutdown()
return
"""
Extract the faultlog at startup
"""
self.flush_rcvd_data=False
rospy.loginfo("Extracting the faultlog")
self.extracting_faultlog = True
if (False == self._extract_faultlog()):
rospy.logerr("Could not get retrieve MOVO faultlog")
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 communication stream")
self.Shutdown()
return
self.start_frequency_samp = True
"""
Force the configuration to update the first time to ensure that the variables are set to
the correct values on the machine
"""
if (False == self._initial_param_force_update()):
rospy.logerr("Initial configuration parameteters my not be valid, please check them in the yaml file")
rospy.logerr("The ethernet address must be set to the present address at startup, to change it:")
rospy.logerr("start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart")
self.Shutdown()
return
rospy.loginfo("Movo Driver is up and running")
class MovoDriver:
def __init__(self,movo_ip='10.66.171.5'):
"""
Variables to track communication frequency for debugging
"""
self.summer=0
self.samp = 0
self.avg_freq = 0.0
self.start_frequency_samp = False
self.need_to_terminate = False
self.flush_rcvd_data=True
self.update_base_local_planner = False
self.last_move_base_update = rospy.get_time()
"""
Initialize the publishers for MOVO
"""
self.movo_data = MOVO_DATA()
"""
Start the thread for the linear actuator commands
"""
self._linear = LinearActuator(movo_ip)
if (False == self._linear.init_success):
rospy.logerr("Could not initialize the linear actuator interface! exiting...")
return
"""
Initialize faultlog related items
"""
self.is_init = True
self.extracting_faultlog = False
"""
Initialize the dynamic reconfigure server for MOVO
"""
self.param_server_initialized = False
self.dyn_reconfigure_srv = Server(movoConfig, self._dyn_reconfig_callback)
"""
Wait for the parameter server to set the configs and then set the IP address from that.
Note that this must be the current ethernet settings of the platform. If you want to change it
set the ethernet settings at launch to the current ethernet settings, power up, change them, power down, set the
the ethernet settings at launch to the new ones and relaunch
"""
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == self.param_server_initialized):
r.sleep()
if (False == self.param_server_initialized):
rospy.logerr("Parameter server not found, you must pass an initial yaml in the launch! exiting...")
return
"""
Create the thread to run MOVO communication
"""
self.tx_queue_ = multiprocessing.Queue()
self.rx_queue_ = multiprocessing.Queue()
self.comm = IoEthThread((movo_ip, 8080),
self.tx_queue_,
self.rx_queue_,
max_packet_size=1248)
if (False == self.comm.link_up):
rospy.logerr("Could not open socket for MOVO...")
self.comm.close()
return
"""
Initialize the publishers and subscribers for the node
"""
self.s = [0]*4
self.faultlog_pub = rospy.Publisher('/movo/feedback/faultlog', Faultlog, queue_size=10,latch=True)
self.s[0] = rospy.Subscriber("/movo/cmd_vel", Twist, self._add_motion_command_to_queue)
self.s[1] = rospy.Subscriber("/movo/gp_command",ConfigCmd,self._add_config_command_to_queue)
self.s[2] = rospy.Subscriber("/move_base/DWAPlannerROS/parameter_updates",Config,self._update_move_base_params)
self.s[3] = rospy.Subscriber("/movo/motion_test_cmd",MotionTestCmd,self._add_motion_test_command_to_queue)
"""
Start the receive handler thread
"""
self.terminate_mutex = threading.RLock()
self.last_rsp_rcvd = rospy.Time.now().to_sec()
self._rcv_thread = threading.Thread(target = self._run)
self._rcv_thread.start()
"""
Start streaming continuous data
"""
rospy.loginfo("Stopping the data stream")
if (False == self._continuous_data(False)):
rospy.logerr("Could not stop MOVO communication stream")
self.Shutdown()
return
"""
Extract the faultlog at startup
"""
self.flush_rcvd_data=False
rospy.loginfo("Extracting the faultlog")
self.extracting_faultlog = True
if (False == self._extract_faultlog()):
rospy.logerr("Could not get retrieve MOVO faultlog")
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 communication stream")
self.Shutdown()
return
self.start_frequency_samp = True
"""
Force the configuration to update the first time to ensure that the variables are set to
the correct values on the machine
"""
if (False == self._initial_param_force_update()):
rospy.logerr("Initial configuration parameteters my not be valid, please check them in the yaml file")
rospy.logerr("The ethernet address must be set to the present address at startup, to change it:")
rospy.logerr("start the machine; change the address using rqt_reconfigure; shutdown; update the yaml and restart")
self.Shutdown()
return
rospy.loginfo("Movo Driver is up and running")
def Shutdown(self):
with self.terminate_mutex:
self.need_to_terminate = True
rospy.loginfo("Movo Driver has called the Shutdown method, terminating")
for i in range(len(self.s)):
self.s[i].unregister()
self.faultlog_pub.unregister()
self.movo_data.Shutdown()
self._linear.Shutdown()
self.comm.Close()
self.tx_queue_.close()
self.rx_queue_.close()
def _run(self):
while not self.need_to_terminate:
"""
Run until signaled to stop
Perform the actions defined based on the flags passed out
"""
result = select.select([self.rx_queue_._reader],[],[],0.02)
if len(result[0]) > 0:
data = result[0][0].recv()
with self.terminate_mutex:
if not self.need_to_terminate:
self._handle_rsp(data)
def _add_command_to_queue(self,command):
"""
Create a byte array with the CRC from the command
"""
cmd_bytes = generate_cmd_bytes(command)
"""
Send it
"""
self.tx_queue_.put(cmd_bytes)
def _update_rcv_frq(self):
if (True == self.start_frequency_samp):
self.samp+=1
self.summer+=1.0/(rospy.Time.now().to_sec() - self.last_rsp_rcvd)
self.avg_freq = self.summer/self.samp
self.last_rsp_rcvd = rospy.Time.now().to_sec()
def _handle_rsp(self,data_bytes):
if (True == self.flush_rcvd_data) or (True == rospy.is_shutdown()):
return
if (self.extracting_faultlog):
valid_data = validate_response(data_bytes,((NUMBER_OF_FAULTLOG_WORDS+1)*4))
else:
valid_data = validate_response(data_bytes,((NUMBER_OF_MOVO_RSP_WORDS+1)*4))
if (False == valid_data):
rospy.logerr("bad movo data packet")
return
rsp_data = array.array('I',data_bytes.tostring()).tolist()
rsp_data = rsp_data[:(len(rsp_data)-1)]
if (self.extracting_faultlog):
self.extracting_faultlog = False
faultlog_msg = Faultlog()
faultlog_msg.data = rsp_data
self.faultlog_pub.publish(faultlog_msg)
else:
header_stamp = self.movo_data.status.parse(rsp_data[START_STATUS_BLOCK:END_STATUS_BLOCK])
wheel_circum = self.movo_data.config_param.parse(rsp_data[START_APP_CONFIG_BLOCK:END_FRAM_CONFIG_BLOCK],header_stamp)
self.movo_data.auxiliary_power.parse(rsp_data[START_BATTERY_DATA_BLOCK:END_BATTERY_DATA_BLOCK],header_stamp)
self.movo_data.propulsion.parse(rsp_data[START_PROPULSION_DATA_BLOCK:END_PROPULSION_DATA_BLOCK],header_stamp)
self.movo_data.dynamics.parse(rsp_data[START_DYNAMICS_DATA_BLOCK:END_DYNAMICS_DATA_BLOCK],header_stamp,wheel_circum)
self.movo_data.imu.parse_data(rsp_data[START_IMU_DATA_BLOCK:END_IMU_DATA_BLOCK],header_stamp)
self._update_rcv_frq()
rospy.logdebug("feedback received from movo")
def _add_motion_command_to_queue(self,command):
"""
Add the command to the queue, platform does command limiting and mapping
"""
cmds = [MOTION_CMD_ID,[convert_float_to_u32(command.linear.x),
convert_float_to_u32(command.linear.y),
convert_float_to_u32(command.angular.z)]]
self._add_command_to_queue(cmds)
def _add_config_command_to_queue(self,command):
try:
cmds = [GENERAL_PURPOSE_CMD_ID,[command_ids[command.gp_cmd],command.gp_param]]
self._add_command_to_queue(cmds)
except:
rospy.logerr("Config param failed, it is probably not known")
return
def _add_motion_test_command_to_queue(self,command):
test = command.test_type & ~MOTION_TEST_TYPE_MASK;
if (0 != test):
rospy.logerr("Bad test command see system_defines.py for details")
cmds = [MOTION_TEST_CMD_ID,
[command.test_type,
command.duration_sec,
convert_float_to_u32(command.magnitude)]]
rospy.loginfo("MOTION_TEST IS GOING TO BE SENT!!!!!!!!!!!!!!")
self._add_command_to_queue(cmds)
def _dyn_reconfig_callback(self,config,level):
"""
The first time through we want to ignore the values because they are just defaults from the ROS
parameter server which has no knowledge of the platform being used
"""
if (True == self.is_init):
self.is_init = False
return config
"""
Create the configuration bitmap from the appropriate variables
"""
config_bitmap = (((config.motion_while_charging^1) << DISABLE_AC_PRESENT_CSI_SHIFT)|
(config.motion_ctl_input_filter << MOTION_MAPPING_FILTER_SHIFT))
"""
Define the configuration parameters for all the platforms
"""
self.valid_config_cmd = [LOAD_MACH_CONFIG_CMD_ID,
[convert_float_to_u32(config.x_vel_limit_mps),
convert_float_to_u32(config.y_vel_limit_mps),
convert_float_to_u32(config.accel_limit_mps2),
convert_float_to_u32(config.decel_limit_mps2),
convert_float_to_u32(config.dtz_decel_limit_mps2),
convert_float_to_u32(config.yaw_rate_limit_rps),
convert_float_to_u32(config.yaw_accel_limit_rps2),
convert_float_to_u32(config.wheel_diameter_m),
convert_float_to_u32(config.wheel_base_length_m),
convert_float_to_u32(config.wheel_track_width_m),
convert_float_to_u32(config.gear_ratio),
config_bitmap]]
rospy.loginfo("Reconfigure Requested!")
rospy.loginfo("x_vel_limit_mps: %f"%config.x_vel_limit_mps)
rospy.loginfo("y_vel_limit_mps: %f"%config.y_vel_limit_mps)
rospy.loginfo("accel_limit_mps2: %f"%config.accel_limit_mps2)
rospy.loginfo("decel_limit_mps2: %f"%config.decel_limit_mps2)
rospy.loginfo("dtz_decel_limit_mps2: %f"%config.dtz_decel_limit_mps2)
rospy.loginfo("yaw_rate_limit_rps: %f"%config.yaw_rate_limit_rps)
rospy.loginfo("yaw_accel_limit_rps2: %f"%config.yaw_accel_limit_rps2)
rospy.loginfo("wheel_diameter_m: %f"%config.wheel_diameter_m)
rospy.loginfo("wheel_base_length_m: %f"%config.wheel_base_length_m)
rospy.loginfo("wheel_track_width_m: %f"%config.wheel_track_width_m)
rospy.loginfo("gear_ratio: %f"%config.gear_ratio)
rospy.loginfo("motion_while_charging: %u"%config.motion_while_charging)
rospy.loginfo("motion_ctl_input_filter: %u"%config.motion_ctl_input_filter)
rospy.loginfo("strafe_correction_factor: %u"%config.strafe_correction_factor)
rospy.loginfo("yaw_correction_factor: %u"%config.yaw_correction_factor)
"""
The teleop limits are always the minimum of the actual machine limit and the ones set for teleop
"""
config.teleop_x_vel_limit_mps = minimum_f(config.teleop_x_vel_limit_mps, config.x_vel_limit_mps)
config.teleop_y_vel_limit_mps = minimum_f(config.teleop_y_vel_limit_mps, config.y_vel_limit_mps)
config.teleop_accel_limit_mps2 = minimum_f(config.teleop_accel_limit_mps2, config.accel_limit_mps2)
config.teleop_yaw_rate_limit_rps = minimum_f(config.teleop_yaw_rate_limit_rps, config.yaw_rate_limit_rps)
config.teleop_yaw_accel_limit_rps2 = minimum_f(config.teleop_yaw_accel_limit_rps2, config.yaw_accel_limit_rps2)
config.teleop_linear_actuator_vel_limit = minimum_f(config.teleop_linear_actuator_vel_limit, config.linear_actuator_vel_limit_mps)
"""
Set the teleop configuration in the feedback
"""
self.movo_data.config_param.SetTeleopConfig([config.teleop_x_vel_limit_mps,
config.teleop_y_vel_limit_mps,
config.teleop_accel_limit_mps2,
config.teleop_yaw_rate_limit_rps,
config.teleop_yaw_accel_limit_rps2,
config.teleop_arm_vel_limit,
config.teleop_pan_tilt_vel_limit,
config.teleop_linear_actuator_vel_limit])
"""
Update the linear actuator velocity limit
"""
self._linear.UpdateVelLimit(config.linear_actuator_vel_limit_mps)
if self.param_server_initialized:
if ((1<<4) == (level & (1<<4))):
rospy.sleep(0.1)
cmds = [GENERAL_PURPOSE_CMD_ID,
[6,convert_float_to_u32(config.strafe_correction_factor)]]
self._add_command_to_queue(cmds)
rospy.sleep(0.1)
if ((1<<5) == (level & (1<<5))):
rospy.sleep(0.1)
cmds = [GENERAL_PURPOSE_CMD_ID,
[7,convert_float_to_u32(config.yaw_correction_factor)]]
self._add_command_to_queue(cmds)
rospy.sleep(0.1)
"""
Check and see if we need to store the parameters in NVM before we try, although the NVM is F-RAM
with unlimited read/write, uneccessarily setting the parameters only introduces risk for error
"""
if self.param_server_initialized:
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES):
if (self.movo_data.config_param.configuration_feedback[i] != self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
rospy.loginfo("Sent config update command")
self.param_server_initialized = True
self.valid_config = config
self.update_base_local_planner = True
self._update_move_base_params(None)
return config
def _update_move_base_params(self,config):
"""
If parameter updates have not been called in the last 5 seconds allow the
subscriber callback to set them
"""
if ((rospy.Time.now().to_sec()-self.last_move_base_update) > 5.0):
self.update_base_local_planner = True
if self.update_base_local_planner:
self.update_base_local_planner = False
self.last_move_base_update = rospy.Time.now().to_sec()
try:
dyn_reconfigure_client= Client("/move_base/DWAPlannerROS",timeout=1.0)
changes = dict()
changes['acc_lim_x'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_y'] = maximum_f(self.valid_config.accel_limit_mps2, self.valid_config.decel_limit_mps2)
changes['acc_lim_th'] = self.valid_config.yaw_accel_limit_rps2
changes['max_vel_x'] = self.valid_config.x_vel_limit_mps
changes['max_vel_y'] = self.valid_config.y_vel_limit_mps
changes['max_rot_vel'] = self.valid_config.yaw_rate_limit_rps
dyn_reconfigure_client.update_configuration(changes)
dyn_reconfigure_client.close()
except:
pass
rospy.loginfo("Movo Driver updated move_base parameters to match machine parameters")
def _continuous_data(self,start_cont):
set_continuous = [GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_CONTINUOUS_DATA,start_cont]]
ret = False
if (True == start_cont):
r = rospy.Rate(10)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.movo_data.status.init):
self._add_command_to_queue(set_continuous)
r.sleep()
ret = not self.movo_data.status.init
else:
r = rospy.Rate(5)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (False == ret):
self._add_command_to_queue(set_continuous)
rospy.sleep(0.6)
if ((rospy.Time.now().to_sec() - self.last_rsp_rcvd) > 0.5):
ret = True
r.sleep()
self.movo_data.status.init = True
return ret
def _extract_faultlog(self):
r = rospy.Rate(2)
start_time = rospy.Time.now().to_sec()
while ((rospy.Time.now().to_sec() - start_time) < 3.0) and (True == self.extracting_faultlog):
self._add_command_to_queue([GENERAL_PURPOSE_CMD_ID,[GENERAL_PURPOSE_CMD_SEND_FAULTLOG,0]])
r.sleep()
return not self.extracting_faultlog
def _initial_param_force_update(self):
"""
Load all the parameters on the machine at startup; first check if they match, if they do continue.
Otherwise load them and check again.
"""
r = rospy.Rate(2)
start_time = rospy.get_time()
params_loaded = False
while ((rospy.get_time() - start_time) < 3.0) and (False == params_loaded):
load_params = False
for i in range(NUMBER_OF_CONFIG_PARAM_VARIABLES):
if (self.movo_data.config_param.configuration_feedback[i] != self.valid_config_cmd[1][i]):
load_params = True
if (True == load_params):
self._add_command_to_queue(self.valid_config_cmd)
r.sleep()
else:
params_loaded = True
return params_loaded