def connect(self): try: self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo) self.__find_motors() except dynamixel_io.SerialOpenError, e: rospy.logfatal(e.message) sys.exit(1)
def connect(self): try: self.dxl_io = dynamixel_io.DynamixelIO(self.port_name, self.baud_rate, self.readback_echo) self.__find_motors() except dynamixel_io.SerialOpenError as e: rospy.logfatal(e.message) sys.exit(1) self.running = True if self.update_rate > 0: Thread(target=self.__update_motor_states).start() if self.diagnostics_rate > 0: Thread(target=self.__publish_diagnostic_information).start()
help= 'connection to serial port will be established at BAUD bps [default: %default]' ) (options, args) = parser.parse_args(sys.argv) if len(args) < 2: parser.print_help() exit(1) port = options.port baudrate = options.baud motor_ids = args[1:] try: dxl_io = dynamixel_io.DynamixelIO(port, baudrate) except RuntimeError, soe: print 'ERROR:', soe else: responses = 0 print 'Pinging motors:' for motor_id in motor_ids: motor_id = int(motor_id) print '%d ...' % motor_id, if dxl_io.ping(motor_id): responses += 1 values = dxl_io.get_feedback(motor_id) angles = dxl_io.get_angle_limits(motor_id) model = dxl_io.get_model_number(motor_id) firmware = dxl_io.get_firmware_version(motor_id) values['id'] = motor_id