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)
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()
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 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